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ABSTRACT 


This  thesis  presents  a  method  of  controlling  an  autonomous  vehicle’s  motion  in  a  two 
dimensional  environment.  Its’  puipose  is  to  expand  the  functionality  of  a  vehicle’s  motion 
by  complementing  a  point  to  point  path  planning  scheme  with  a  path  to  path  scheme.  The 
method  introduced  in  this  paper  will  use  the  vehicle’s  position  and  the  desired  path  to 
calculate  the  necessary  curvature  to  effect  movement  onto  the  desired  reference  path.  The 
reference  path  will  be  a  simple  planar  curve,  such  as,  a  circle  or  line.  After  successful 
testing  of  an  operating  algorithm,  the  method  shall  be  incorporated  into  a  robot’s  software 
system.  This  path  tracking  method  will  lay  the  groundwork  for  a  dynamic  obstacle 
avoidance  system  for  a  mobile  robot. 
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I.  INTRODUCTION 


A.  BACKGROUND 

Presently,  the  bulk  of  research  conducted  in  the  area  of  path  planning  for  an 
autonomous  vehicle  deals  with  sub-dividing  the  vehicles  desired  motion  into  multiple 
independent  paths.  The  most  prominent  approach  has  been  to  describe  the  desired  motion 
tlirough  a  sequence  of  configurations.  These  configurations  define  the  vehicles  x,  y 
coordinate  position  coupled  with  the  vehicle’s  orientation  [Ref.  1J.  This  method  of  path 
planning  can  be  best  described  as  a  point  to  point  control  scheme.  As  such,  a  path  planning 
algorithm  calculates  the  path  between  adjacent  configurations  to  find  the  independent  legs 
of  the  vehicle's  motion.  This  scheme  reflects  a  wide  spectrum  of  motion  found  within  the 
world,  and  it  enjoys  many  advantages,  such  as;  its’  simplicity,  wide  applicability,  and  ease 
of  manipulation.  However,  there  are  circumstances  in  which  a  point  to  point  scheme  will 
not  fully  replicate  the  desires  of  the  user. 

An  example  can  best  be  illustrated  by  reflecting  on  the  motion  of  a  car.  Assume  a  car 
parked  alongside  the  curb  desires  to  pullout  into  traffic,  proceed  down  the  road  for  two 
blocks,  and  then  make  a  right  turn  at  the  light.  In  this  example  there  are  no  predetermined 
points  which  describe  the  motion,  rather  there  exist  the  concept  of  traffic  lanes  which  define 
and  restrict  the  motion.  Although  a  sequence  of  points  could  define  a  similar  range  of 
motion,  the  point  to  point  scheme  would  require  the  user  to  have  prior  knowledge  of 
transition  points.  In  addition,  the  point  to  point  scheme  places  no  restrictions  on  the  bounds 
of  movement  between  two  adjacent  configurations.  This  could  raise  problems  in  a  dynamic 
environment.  On  the  otherhand,  the  path  tracking  method  which  we  shall  introduce  restricts 
the  vehicle's  motion  to  the  interior  boundaries  set  by  the  intersecting  paths.  Thereby 
increasing  control  over  the  vehicle’s  motion,  while  reducing  the  information  needed  to 
program  a  vehicle.  In  the  path  tracking  method  the  configurations  lose  their  importance  to 
the  more  general  entity  of  the  paths,  which  are  represented  as  traffic  lanes. 
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B.  THESIS  ORGANIZATION 


The  objective  of  this  paper  is  to  develope  a  mathematical  model  to  support  a  path  to 
path  motion  control  system.  The  scope  of  our  study  will  be  limited  to  include  simple  planar 
curves,  such  as  lines  and  circles.  Upon  deriving  a  mathematical  foundation,  we  shall 
translate  the  work  into  a  working  simulator.  This  simulator  should  give  the  user  the 
capability  to  fully  test  the  algorithm,  to  include  merging  a  vehicle  onto  simple  planar  paths, 
and  to  transition  from  path  to  path.  After  successful  development  and  testing,  the  algorithm 
shall  be  incorporated  into  the  motion  control  system  of  an  operating  vehicle,  specifically 
the  Yamabico-1 1  robot. 

The  layout  of  the  thesis  shall  reflect  the  outline  of  the  research.  Chapter  II  presents  a 
detailed  problem  statement  to  include  our  assumptions  and  requirements  for  a  path  tracking 
algorithm.  Chapter  III  will  discuss  the  mathematical  groundwork  necessary  to  develope  our 
system.  In  this  section  we  shall  derive  a  control  function  which  will  be  suitable  to  merge  an 
autonomous  vehicles  onto  a  reference  path.  Chapter  IV  will  then  develope  the  equations 
necessary  to  support  the  control  function.  In  the  development  phase  we  will  separate  our 
study  into  two  cases,  lines  and  circles.  With  the  development  of  an  appropriate  framework. 
Chapter  V  will  discuss  the  inherent  problems  in  transitions  between  paths,  and  the  specifics 
of  our  transition  scheme.  Chapter  VI  will  elaborate  on  the  details  of  implementation  of  the 
path  tracking  algorithm  into  the  software  of  the  Yamabico-1 1  robot.  Finally,  Chapter  VII 
shall  be  a  look  at  future  areas  of  research  and  some  conclusions.  An  appendix  shall  contain 
the  path  tracking  simulator  which  was  written  in  C.  and  a  users  manual  for  operating  the 
path  tracking  algoritlim. 


n.  PROBLEM  STATEMENT 


The  problem  can  be  stated  as  follows:  find  a  smooth  path  from  an  initial  configuration 
p,-  to  a  reference  path  p ref.  The  basic  concept  of  our  method  shall  be  to  vary  the  vehicles 

instantaneous  curvature  in  order  to  manipulate  the  vehicle’s  position  and  heading.  To 
facilitate  this,  we  shall  expand  the  concept  of  a  configuration  to  be  a  quadruple  (x,  y,  0,  K), 
which  describes  the  vehicles  position  by  it’s  cartesian  coordinates  coupled  with  it’s  heading 
and  instantaneous  curvature.  The  reference  path  will  be  defined  by  a  similar  quadruple. 
This  pennits  a  line  or  a  circle  to  be  represented  by  the  same  structure  depending  on  the 
value  of  k.  For  example,  we  might  want  to  solve  the  problem  presented  earlier,  concerning 
the  car  entering  a  traffic  lane  from  a  curb  position  and  then  making  a  right  turn.  The 
vehicles  curb  position  would  be  represented  p,-  =  (0,  0,  0,  0),  while  the  traffic  lanes  would 
be  represented  by  pie^j  =  (0.  10.  0. 0),  and  prey2  =  (50,  0,  nJ2.  0)  as  shown  in  Figure  1 . 


prefI  =(0,10,0,0) 
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Figure  1.  Vehicle  Pulling  Away  From  the  Curb 


In  developing  our  solution  we  shall  ignore  the  size  and  shape  of  the  vehicle  by 
assuming  our  vehicle  is  a  point  robot,  and  where  the  clearance  to  obstacles  has  been 
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assessed  at  a  higher  level  within  the  system.  This  will  simplify  our  work  by  removing  the 
finite  curvature  limitation  and  clearance  requirements  necessary  for  a  rigid  body  robot.  In 
addition  we  shall  assume  the  velocity  of  the  vehicle  is  a  constant  positive  value,  and  that 
the  vehicle's  motion  will  be  controlled  solely  through  altering  the  vehicle’s  instantaneous 
curvature.  These  assumptions  will  restrict  the  research  to  a  spatial  problem  and  alleviate  the 
need  to  consider  the  dynamic  relationships  inheient  to  the  problem,  e.g.,  time,  speed, 
acceleration,  and  rotation.  By  making  these  assumptions  we  will  simplify  the  mathematical 
model  required  in  finding  a  suitable  path  tracking  algorithm. 

In  our  initial  problem  statement  we  required  that  the  proposed  algorithm  produce  a 
smooth  path.  The  derivative  of  curvature  is  the  only  control  variable  within  the 
mathematical  model.  Since  the  derivative  of  the  curvature  dtc/ds  is  finite,  the  resultant 
vehicle's  trajectory  is  “smooth"  in  the  sense  that  the  tangent  orientation,  curvature,  and 
derivative  of  curv  ature  exist  at  every  point  on  the  trajectory  {Ref.  2).  Although,  this  concept 
of  smoothness  is  essential,  our  requirements  must  be  more  stringent.  The  generated  path 
must  be  suitable  to  be  followed  by  a  robot's  mechanical  power  train  system.  However, 
smoothness  is  a  subjective  quality,  which  must  be  clearly  defined  before  attempting  to 
calculate  a  viable  solution.  The  problem  can  be  characterized  by  how  rapidly  we  wish  to 
converge  onto  a  desired  reference  path.  If  we  attempt  to  converge  in  too  short  of  a  distance, 
the  vehicle's  motion  will  be  unstable  and  the  vehicle  will  lose  tracking  precision.  On  the 
otherhand.  if  the  distance  to  converge  is  too  large,  then  the  vehicle’s  motion  will  be 
inhibited  by  a  need  for  excessive  maneuvering  space.  Therefore,  our  method  will 
incorporate  a  distance  constant.  S().  to  regulate  the  smoothness  of  the  generated  paths.  The 

distance  constant  will  balance  the  need  for  rapid  response  with  the  smoothness 
requirement.  It's  value  will  be  dependant  on  the  maneuvering  characteristics  of  the  vehicle, 
and  will  not  adversely  effect  the  mathematical  correctness  of  our  work. 
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III.  METHOD 


In  developing  any  system  the  first  step  is  to  create  a  model  of  the  system  in  order  to 
analyze  the  system.  The  objective  of  this  is  to  translate  the  real  world  mechanics  of  the 
system  into  a  mathematical  theory  which  accurately  describes  the  system.  This  allows  one 
to  fully  explore  the  nature  of  the  problem  and  mathematically  verify-  the  solution.  As  the 
previous  chapter  outlined  our  system,  this  chapter's  purpose  is  to  develope  the  necessary 
mathematical  equations  to  support  our  previously  stated  goals  and  requirements. 

A.  PATH  CONTROL  BY  CURVATURE 

When  deriving  a  mathematical  equation  to  simulate  a  real  world  problem,  it  is  often 
obvious  what  type  of  equation  is  necessary  to  solve  the  problem.  Physical  models  often 
directly  translate  into  applicable  mathematical  models.  However,  in  our  situation  we 
undertook  this  research  without  a  clear  idea  of  the  final  form  of  our  mathematical  model. 
Although  we  feel  it  may  be  possible  to  solve  the  problem  using  traditional  control  theory- 
methods.  we  desire  an  easier  method  which  may  prove  to  yield  equally  powerful  results. 
Therefore,  we  predicated  our  solution  on  the  belief  that  the  problem  could  be  solved  using 
analysis  through  differential  equations. 

In  our  problem  statement  wc  introduced  the  concept  of  the  configuration  as  a 
quadruple,  which  defines  a  vehicle's  positional  status  by  stating  the  vehicle's  present 
coordinate  position,  its’  present  direction  of  motion,  and  a  measure  of  the  change  in 
direction  of  motion.  We  also  defined  our  desired  position  by  way  of  a  reference  path 
defined  by  the  same  variables  as  that  of  the  vehicle's  position.  This  similarity  allows  us  to 
make  quick  accurate  comparisons  between  the  two  quadruples.  Therefore,  we  can  define  a 
function  which  relates  our  desired  motion  with  respect  to  the  initial  and  goal  positions. 
Using  this  idea  consider  controlling  the  vehicle  by  changing  its  instantaneous  curvature  as 
determined  by  a  function  of  the  initial  and  goal  quadruples. 
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(3.1) 


Equation  (3.1)  can  be  simplified  by  using  the  x,  and  y  coordinates  to  determine,  d,  the 
distance  between  the  present  position  and  the  desired  reference  path  position.  However,  to 
accomplish  this  we  must  narrow  the  infinite  number  of  available  points  on  the  reference 
path  down  to  a  specific  point  of  interest.  The  most  logical  point  would  be  the  point  on  the 
path  closest  to  the  vehicle’s  position.  We  shall  define  this  point  to  be  the  image  of  the 
vehicle’s  position,  and  reference  it  as  p jmago-  Therefore,  at  all  times  there  will  be  an  image 
point  which  will  provide  the  positional  information  of  a  theoretical  vehicle  located  and 
tracking  along  the  reference  path.  The  establishment  of  this  point  allows  us  to  quickly 
calculate  the  closest  distance,  d ,  between  the  vehicle’s  position  and  the  reference  path. 
Thereby  equation  (3.1)  becomes 

dK  ,  „  , 

-j-  =  /( K,  0,  d)  (3.2) 


Equation  (3.2)  restates  our  desire  to  control  the  vehicle  through  changing  the 
instantaneous  curvature,  which  is  dependent  on  the  vehicular  position,  n,,  and  the  reference 
path,  prej  The  equation  does  not  show  the  underlying  relationship  between  p,-  and  pref,  or 
how  their  values  effect  the  change  in  curvature.  However,  since  we  are  attempting  to  find 
a  simple  but  powerful  solution,  a  logical  choice  would  be  to  begin  with  a  linear  relationship. 
Thus,  we  propose  the  following  general  class  as  a  steering  function. 


^  =  -[A{PiK-pimage  k)  +B{PiQ-Pimage  6)  +Cd] 


d  K 


—  +A(PiK-pimage  k)  +B{PiQ-PimageQ)  +  Cd  =  0 


(3.3) 

(3.4) 


Where  A,  B,  and  C  are  constants.  This  steering  function  is  a  simple  linear  equation,  in  which 
the  constants  can  be  solved  for  by  using  differential  equations. 
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B.  COEFFICIENTS  BY  APPROXIMATION 

To  find  the  optimal  values  for  A,  B .  and  C.  let  us  analyze  the  simplest  possible  case. 
That  is  the  case  in  which  the  reference  path,  prep  is  the  x-axis  as  shown  in  Figure  2. 


Figure  2.  Vehicle  Merging  Onto  X-axis 

In  this  case,  we  can  simplify  our  steering  equation  due  to  the  following  observations. 


P image 
P  image 


K  =  0 

e  =  o 


d  =  y 


(3.5) 


That  is  since  the  x-axis  is  the  desired  reference  path,  our  linear  equation  can  be  viewed  as 
dependent  only  on  the  vehicle’s  positional  quadruple.  Therefore,  equation  (3.4)  becomes 

dK 

—  +Ak  +  BQ  +  Cv  =  0  (3.6) 

ds 


Since  we  know 


y  =  y  (x) 


0  =  a  tan  (/) 


(V')3  ,  00 5 


(3-7) 

(3.8) 
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t  3/2 

(i  +  (v'n 


_  0  +  (/)' 2) ' 

_rf'v 


(3.10) 


P  =  y'"  ( 1  +  00  2)  2  -  3v'  (y") 2  ( 1  +  (y') 2)  3 
as 

By  equation  (3.6)  to  (3.11)  the  steering  function  becomes, 


nr  /  //  2  // 

v  3y  (y  )  ax 

- - - =  -  - 5  + - - - : 

2  L  /  2  D  /  2  ' 

(l  +  (yV)  ( i  +  (y  )  )  0  +  00) 


(3.11) 


(3.12) 


_  +  M  .  lv+o,  =  0 


However,  this  equation  is  too  confusing,  and  as  a  result  very  difficult  to  solve.  If  we 
are  to  maintain  a  simple  solution,  we  must  make  some  assumptions,  concerning  the  terms 
of  equation(3.12).  To  accomplish  this  we  shall  make  use  of  two  reasonable  assumptions. 


(v')2«i 

y'(y")2«y'" 


(3.13) 


Both  assumptions  deal  with  the  relative  magnitude  of  the  first  three  derivatives  of  y.  If  these 
assumptions  hold,  which  by  all  indications  they  do  when  the  value  of  x  is  relatively  high, 
they  would  significantly  simplify  our  control  equation.  Using  the  assumptions  of  (3.13), 
equation  (3.12)  becomes, 

v"'  +  Ay"  +  By'  +  Cy  =  G  (3.14) 


Written  in  differential  notation,  we  have 


(. D3  +  AD2+BD  +  C)y  =  C 


(3.15) 


Our  steering  function  thus  becomes  an  ordinary  third  order  differential  equation.  Since 
the  steering  function  is  of  the  third  order,  the  equation  must  have  at  least  one  real  toot.  This 
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real  loot  can  be  either  a  positive  or  a  negative  value.  However,  if  the  real  root  is  a  positive 
value  the  generated  solution  would  diverge  from  the  reference  path.  Therefore,  to  generate 
a  converging  solution,  the  real  root  must  be  a  negative  value.  Let’s  assume  that  the  value 
of  the  root  is  -k.  Then  equation  (3.15)  becomes, 

(-k)3  +  A  (-k)2  +  B  (~k)  +  C  =  C  (3.16) 

By  solving  this  equation  for  the  constants,  and  substituting  their  value  back  into  the 
equation,  we  get 

(D3+AD2  +  BD  +  k3 -Ak2  +  Bk)v  =  C  (3.17) 

(D  +  k)  [ D 2  +  (A-  k)  D  +  k3  -  Ak2  +  Bk]  y  =  0  (3.18) 

The  second  order  polynomial  of  equation  (3.18)  has  two  roots.  If  these  roots  are 
imaginary,  then  the  solution  would  be  oscillatory  and  inappropriate  for  our  goal.  Therefore, 
we  restrict  the  roots  of  this  polynomial  to  be  negative  real  roots,  which  we  shall  assume  to 
be  -A  j  and  -A'2-  Equation  (3.18)  now  becomes 

(D  +  k)  (D  +  k\)  (D  +  ki)y  =  0  (3.19) 

Since,  there  are  no  advantages  of  having  three  distinct  values  for  k,  k q,  and  k2.  we  shall 
assume  the  three  roots  are  the  same. 

k  =  k\  =  A-2  (3.20) 

Therefore,  equation  (3.19)  becomes 

(D  +  k)3y  =  0  (3.21) 

Now.  solving  for  y  we  get 

v  =  (^x1  +  Bx  +  C)  E~kx  (3.22) 

Using  equations  (3.15  and  3.21)  we  can  solve  the  equation  for  the  values  of  the 
constants  A,  B,  and  C.  This  is  simply  accomplished  by  expanding  the  third  order 
polynomial  and  individually  solving  the  coefficient  for  each  order. 

D3  +  AD 2  +  BD  +  C  =  (D  +  k) 3  (3.23) 
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(3.24) 


D3  +  AD2  +  BD  +  C  s  £>3  +  3  kD2  +  3  k2D  +  k~ 

Therefore,  the  value  of  the  constants  are  as  follows. 

A  =  3k 

B  =  3  A-2  (3.25) 

C  =  A3 

Now  that  we  have  calculated  the  values  of  the  individual  constants  we  can  complete 
our  steering  function  by  substituting  these  values  into  equation  (3.3). 

Ys  =  -^k<PiK-PimagcK)  +3 A2  (PjQ-pimageQ)  +k 3d]  (3.26) 

Equation  (3.26)  is  the  final  fonn  of  our  derived  curvature  control  equation.  However,  we 
still  must  determine  an  appropriate  value  for  the  constant  k.  We  know  that  the  coefficients 
of  the  derived  equation  will  affect  the  responsiveness  and  smoothness  of  the  path  generated 
by  the  steering  function.  Furthermore,  we  earlier  established  the  distance  constant,  SQ.for 
the  same  purpose.  Thus  a  logical  solution  would  be  to  base  the  value  of  k  on  the  value  of 
S(,.  Therefore,  for  our  algorithm  we  shall  assign  the  value  of  k  to  be  the  inverse  of  S0. 


(3.27) 


This  completes  the  derivation  of  our  steering  function.  The  output  of  equation  (3.26)  should 
generate  a  smooth  path  which  meets  our  stated  requirements.  Preliminary  results  of  the 
steering  function  are  illustrated  in  Figures  3  and  4.  The  output  displayed  in  Figure  3  depicts 
the  generated  paths  for  various  initial  configurations  and  the  x-axis  being  the  reference 
path.  In  each  of  the  cases  the  initial  configuration  is  the  point  pj  =  (0,1 )  while  the  vehicle’s 
orientation  is  the  intervals  of  every  45  degrees.  On  the  otherhand,  Figure  4  illustrates  the 
effects  of  changing  the  value  Sq  has  on  the  path  generated  by  the  algorithm.  The  output 
illustrated  in  Figures  3  and  4  are  similar  to  what  we  generally  expected,  and  fully  meet  our 
requirements.  However,  to  verify  the  output  and  the  equation’s  suitability,  we  must  check 
it  through  experimental  results. 
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C.  EXPERIMENTAL  RESULTS 


Ill  deriving  our  curvature  control  equation  we  made  some  assumption  vital  to 
simplifying  our  equation.  These  assumptions  dealt  with  the  relative  magnitude  of  the 
derivatives  of  the  v  component  of  the  vehicle’s  position,  and  the  values  of  the  roots  of  a 
third  order  polynomial.  Whenever  assumptions  are  made  within  a  mathematical  problem 
error  is  introduced  into  the  final  solution.  In  order  to  validate  the  solution  and  the 
assumptions,  we  must  offer  some  substantiative  proof  that  the  work  was  not  drastically 
flawed  by  making  the  assumptions.  The  best  proof  is  sound  mathematical  support  for  the 
assumptions.  We  can  not  offer  this  in  our  case.  However,  we  can  support  our  assertions  via 
experimental  results. 

If  we  can  show  that  the  results  of  our  curvature  control  equation  closely  match  the 
output  of  a  similar  system  which  does  not  take  our  assumptions  in  consideration,  then  we 
add  a  degree  of  validity  to  our  work.  Thus,  we  intend  to  verify  our  work  by  comparing  the 
results  of  our  algorithm  to  the  results  of  an  equation  derived  without  our  assumptions.  Thus, 
we  shall  compare  the  output  results  of  our  curvature  control  function  of  equation  (3.26)  to 
the  assumption  free  equation(3.22).  To  accomplish  this  we  shall  make  the  comparison  for 
a  specific  problem.  This  problem  will  be  the  case  of  merging  a  vehicle  with  a  initial  position 
Pi  =  (0,1, 0,0)  onto  the  directed  line,  pref  =  (0,0, 0,0),  which  is  simply  the  x-axis.  By  using 
this  case  we  can  easily  calculate  the  coefficients  for  equation  (3.22).  By  solving  for  the 
variables  and  using  variable  substitution  we  calculated  the  coefficients'  values; 

A  -  k2yQ 

D  =  kyQ  (3.28) 

C  =  -v0 

Details  of  the  actual  mathematical  process  involved  can  be  found  in  appendix  A.  Using 
these  values  in  the  assumption  free  control  function  we  can  compare  results. 

We  conducted  four  separate  cases  for  comparison  using  the  problem  as  stated  above. 
In  each  case  we  solved  the  problem  of  merging  a  vehicle  onto  the  x-axis.  while  we  varied 
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the  value  of  S(>  for  each  test  case.  The  graphical  results  of  both  oui  curvature  control 
algorithm  and  the  actual  output  as  found  by  the  assumption  free  function  is  depicted  in 
Figures5  to  8.  The  results  of  the  comparison  are  very  good.  In  our  results  we  see  that  the 
output  of  our  path  tracking  algorithm  are  very  close  to  the  actual  values.  The  test  results 
show,  that  as  we  increase  the  distance  constant,  Sy,  the  difference  in  the  output  becomes 
even  less  significant,  which  meets  our  expectations.  Although,  the  difference  increases  as 
the  distance  constant  is  significantly  decreased,  as  in  the  case  of  Sy  =  0.125.  the  output  is 
not  inappropriate.  The  results  show,  that  at  no  instance  does  an  extreme  difference  in  the 
output  between  the  two  functions  exist.  Therefore,  the  results  fully  support  the  assumption 
made  within  our  work.  Although,  this  does  not  fully  validate  our  results,  it  does  provide  a 
significant  degree  of  confidence  in  our  method.  With  this  degree  of  confidence  we  have 
faith  that  we  are  on  the  correct  track,  and  that  our  algorithm  is  appropriate. 

D.  SUMMARY 

In  this  chapter  we  laid  the  mathematical  groundwork  for  our  path  tracking  algoritlun. 
Initially,  we  had  few  concrete  ideas  on  the  form  of  our  controlling  equation.  However,  our 
guiding  factor  was  to  keep  the  equation  simple  and  powerful.  This  lead  us  to  guess  the 
format  of  a  suitable  equation  to  be  a  linear  differential  equation.  From  this  we  generated  a 
simple  steering  function  based  on  the  difference  between  the  vehicle’s  position  and  the 
vehicle’s  image  position  on  a  reference  path.  The  resulting  differential  equation  was 
cumbersome  and  difficult  to  solve.  Therefore,  we  made  some  assumption  about  the  relative 
values  of  the  various  derivatives  of  v.  This  enabled  us  to  reduce  the  problem  to  a  third  order 
differential  equation,  which  could  be  solved  rather  easily.  Upon  solving  the  differential 
equation.  w>e  proposed  using  Sy  as  the  basis  for  the  coefficients  of  the  resulting  equation. 

Thereby,  allowing  us  to  balance  the  need  for  responsiveness  with  the  requirement  of 
smoothness.  The  paths  generated  by  the  proposed  steering  function  fully  meet  our 
expectations.  In  addition,  we  tested  our  results  by  making  a  comparison  between  the  output 
of  our  derived  curvature  control  function  and  an  assumption  free  version  of  the  steering 
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function.  The  graphical  results  showed  no  significant  deviations  or  aberrant  trends  between 
the  two  functions  output.  Thereby,  these  experiments  fully  support  the  appropriateness  of 
our  equation,  lending  a  degree  of  validity  to  our  path  tracking  method. 
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IV.  DETAILED  ALGORITHMS 


Chapter  III  successfully  derived  a  curvature  control  equation  for  our  path  tracking 
algorithm.  In  this  chapter  we  shall  develope  the  support  equations  necessary  to  incorporate 
the  control  equation  into  a  broader  algorithm  for  path  tracking.  The  overall  objective  for  the 
algorithm  is  to  alter  a  vehicle’s  location  defined  by  a  configuration  onto  a  planar  curve.  To 
accomplish  this  we  shall  undertake  three  steps:  to  calculate  the  closest  distance  between  p,- 
and  p rcf'  to  locate  p image*  and  to  calculate  the  change  in  curvature.  With  these  steps 
accomplished  we  can  construct  a  program  which  would  successfully  maneuver  a  vehicle 
onto  any  given  reference  path. 

A.  CLOSEST  DISTANCE 

Tire  first  step  in  our  general  scheme  is  to  calculate  the  shortest  distance  between  pf-  and 
p,  cf.  We  shall  refer  to  this  distance  as  d ciose.  There  are  numerous  methods  to  calculate 
distance.  However,  in  our  case  we  desire  a  method  which  will  be  versatile  enough  to 
calculate  this  distance  given  configurations,  which  may  either  represent  a  point,  a  line,  or  a 
circle.  We  do  not  know  of  an  equation  which  will  allow  us  to  perform  such  a  calculation. 
Therefore,  we  shall  derive  a  distance  equation  flexible  enough  to  meet  our  requirements. 
There  are  two  possible  cases  in  our  model;  point  to  line,  and  point  to  circle.  We  shall  look 
at  these  cases  separately,  and  derive  suitable  equations  for  each  case. 

I.  Point  to  Line 

Our  objective  is  to  calculate  the  shortest  distance  between  any  directed  line  pref = 
(a,  />,  a)  and  a  point  pj  =  {x,  y).  Thus  dist{ pref,  p;)  refers  to  the  perpendicular  distance 
between  the  point  and  the  line.  In  preparation  for  this  calculation,  let’s  calculate  the 
distance  between  the  point  of  interest  and  a  special  directed  line,  py  =  (0, 0,  a).  This  directed 
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line  is  simply  a  line  parallel  to  pref,  which  originates  at  the  origin.  As  Figure  9  illustrates, 
using  simple  trigonometry  the  distance  is 

distipQ'Pj )  =  vcos(a)  -Asin(a)  (4.1) 

Using  the  results  of  equation  (4.1)  we  can  generate  a  general  distance  equation. 
This  general  equation  will  be  the  difference  between  the  distance  from  the  point  to  the 
refeience  path  and  the  distance  from  the  reference  path  to  our  special  directed  line.  This 
general  equation  is  illustrated  in  Figure  10.  To  calculate  the  difference  we  simply  apply 
equation  (4.1)  twice. 

distiPrtfP,)  =  dist{po,pt)  -  distip^p,.^  (4.2) 

=  y cos  (a)  --vsin(a)  -b cos  (a)  +asin(a)  (4.3) 

=  (y  -  b)  cos  ( a)  -  (.v  -  a)  sin  ( a)  (4.4) 

Note,  that  dist  <  0  if  the  orientation  between  the  directed  line  and  the  point  is  clockwise. 
Thus,  this  provides  us  with  a  suitable  equation  for  the  shortest  distance  between  a  point  and 
any  directed  line. 

2.  Point  to  Circle 

In  deriving  the  shortest  distance  from  a  point  pj  =  (.v,y)  to  a  circle  pref  =  (*,)', 0,  k), 
we  must  realize  that  two  cases  exist.  The  value  of  K  can  be  either  positive  or  negative.  We 
shall  examine  these  two  cases  separately.  Both  cases  are  illustrated  in  Figure  11  and  12, 
respectively. 

a.  Positive  Curvature 

Let’s  first  calculate  the  origin  of  the  reference  circle.  This  can  be  done  by 
adding  the  x,  and  y  components  of  the  radius  to  the  x,  and  y  coordinates  of  the  reference 
circle. 
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Figure  9.  Distance  Between  Pj  and  P0 
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Figure  11.  Positive  Curvature  Case 


Figure  12.  Negative  Curvature  Case 
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(4.5) 


fy  =  P0  +  (^-)  cos  (0O  + 71/2) 
K0 

>V  =Pq+  (“)  sin  (0O  + 7i/2) 

Ko 


Cvyf)  =  (aq-  (  —  )  sin  (0. )  ,y0  +  ( -J- )  cos  (0, ) )  (4.6) 

K()  K0 

Now,  that  we  have  the  coordinates  of  the  origin,  we  can  calculate  the  distance  between  the 
origin  and  the  point  by  using  Euler’s  distance  equation.  Furthermore,  since  we  know  the 
distance  between  the  origin  and  the  reference  path,  the  radius,  we  can  calculate  the  distance 
between  pj  and  pref.  This  is  accomplished  by  taking  the  difference  between  the  two 
calculated  distances. 


dist(prefPj)  =  radius  ~  disr(porjgjn,Pj) 


(4.7) 


Kq  ai 


sin0. 


a  -  |a0- 


2  ^ 


y-  >’o 


COS0, 


(4.8) 


We  can  simplify  equation  (4.8)  by  multiplying  through  with  an  appropriate  factor. 


(  — ) 
K(» 


(A  -  Aq)  + 


sin0. 


K„ 


COS0„ 


°"3'o)  Z 


Ko  +  <l 


sin  0^1 


O'-.V+nr 


COSOyl 


By  factoring  we  get. 


(  2sin0,.\  /  zcoswn\ 

-  (X-X0)  ^-,0 - — J-  (>’  —  >'„)  - T-J 


0  J 


2cos0„ 


_L  ^ 

K()  +  ^ 


(A-A0)  + 


sin  0^1 


K 


0  J 


COS0r 


(y  -  y0)  + 


(4.9) 


(4.10) 


Finally,  we  can  multiply  by  Kq/Kq  to  simplify  equation  (4.10). 
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(4.11) 


-  (.v  —  ,v0)  [k0(,y-.y())  +2sin0o]  -  (v-y0)  Ik0(v-v0)  -2cos0()J 


1  +  tj  [  Kq  (.y  —  a())  +sin0o]“+  [k0(v-  v0)  -cos0oJ" 

This  provides  us  with  the  final  fonn  of  our  equation.  It  will  calculate  the  distance  between 
any  point  and  any  positive  curvature  circle.  Also  note  that  with  this  equation  dist( pref,  p,)  < 
0  if  the  point  p,  is  not  circumscribed  by  the  circle. 


b.  Negative  Curvature 

Figure  10  illustrates  the  situation  when  pref  curvature  is  negative.  As  this 
situation  is  veiy  similar  to  the  case  with  a  positive  curvature,  we  use  the  same  method  to 
calculate  the  dist{ pref,  pj).  Howevei.  since  the  curvature  of  the  circle  is  negative,  this  will 


give  us  a  negative  value  for  the  radius.  Thus,  in  this  C’«e  we  subtract  the  length  of  the  radius 
from  the  distance  between  the  origin  and  pj. 


dist{prcfPi 


sin0 


V° 


y-  vo 


COS0, 


K„ 


(4.12) 


We  can  simplify  this  equation  by  the  method  used  earlier  for  the  positive  curvature  case. 
The  results  of  this  method  will  produce  the  same  equation  as  the  positive  curvature  case. 
However,  in  this  case  dist( pref,  pj)  <  0  when  p,  is  circumscribed  by  the  circle. 


3.  General  Distance  Equation 

Our  objective  was  to  derive  a  single  general  equation  to  calculate  the  shortest 
distance  between  a  point  and  a  reference  path.  However,  it  seems  we  have  derived  two 
separate  equations  to  fulfill  our  needs.  Yet,  if  we  take  a  closer  look  at  the  two  equations  we 
shall  reveal  an  interesting  fact.  We  can  consider  a  directed  line,  a  special  case  of  a  circle 
with  an  infinite  radius.  The  line’s  curvature  is  defined  to  be  equal  to  zero.  Therefore,  if  we 
implement  equation  (4.1 1 )  in  the  case  of  a  directed  line,  we  get, 

-  (,y-.y0)  [k0(.y-.y0)  +2sin0oj  -  (v-y0)  [k0(v-v0)  -2cos0oJ 
dist=  - ;= —  = — ===== — -  =  ==  = - (4.13) 

1  +,/[K0(.Y-.Y0)  +SU10()]2+  (  Ky  ( y  -  y0)  -COS0()12 
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(4.14) 


-  (.y-a0)  [2sin0o]  -  ( v  —  Vq)  [-2cos90] 

1  +  J[sin0()]i+  [-cos0o]2 

=  -  (a  -  a())  sin0o  +  ( v  -  >-0)  cos0o  (4.15) 

This  suggests  that  the  distance  equation  derived  in  the  case  that  the  reference  path  is  a  circle 
subsumes  both  possible  reference  path  cases.  Thus,  we  have  established  a  single  general 
equation  to  determine  the  shortest  distance  between  any  point  and  any  simple  planar 
reference  curve. 


B.  THE  IMAGE 

The  control  equation  derived  in  chapter  III  was  predicated  on  the  establishment  of  a 
point  referred  to  as  Piniage-  That  is  the  configuration  of  a  theoretical  vehicle,  which  is 
continuously  maintaining  position  along  the  reference  path.  This  configuration  includes  the 
theoretical  vehicle’s  coordinate  position,  orientation,  and  curvature.The  difficulty  in 
calculating  Pimage  is  that  the  reference  path  could  be  either  a  line  or  a  circle.  Unfortunately, 
there  is  not  a  single  method,  to  our  knowledge,  that  would  suffice  for  both  of  these  cases. 
Therefore,  we  must  derive  two  unique  methods  to  calculate  Piniage  To  accomplish  this  we 
shall  address  the  two  cases  separately. 


1.  Lines 


The  case  of  finding  pjmage  for  a  vehicle  on  a  directed  line  is  quite  simply.  Since 
the  curvature  of  a  directed  line  is  defined  to  be  equal  to  zero,  and  the  orientation  of  all  points 
on  a  directed  line  are  defined  to  be  equal  to  the  orientation  of  the  directed  line  itself,  the 
calculation  of  page's  curvature  and  orientation  is  trivial.  That  is. 


Pimage*  =Prefi 
P  image K  ~  ® 


(4.16) 


Thus,  in  the  case  of  the  reference  path  being  a  line,  to  calculating  Pimage  is  reduced  to 
calculating  the  x,  and  y  coordinates  of  Pimage-  Basically,  to  accomplish  this  we  need  to  find 


26 


the  shortest  distance  between  the  vehicle’s  position  and  the  directed  line.  This  situation  is 
illustrated  in  Figure  13. 


Figure  13.  Calculating  Pimage  for  a  Line 

To  calculate  the  shortest  distance  between  pj  and  pref  we  use  equation  (4.11), 
which  was  developed  as  a  general  equation  for  calculating  the  closest  distance.  However, 
we  can  simplify  this  equation  since  the  curvature  of  a  line  is  defined  as  zero.  Therefore,  the 
equation  for  the  shortest  distance  between  a  point  and  a  directed  line  as  defined  with 
configurations  is, 

d d0se  =  ( Pt )'  ~  Pref >*)  cos  ( a)  “  iPf*  “  Prcfx)  sin  0 a)  <4- 1 7) 

Given  the  closest  distance  we  can  now  calculate  the  coordinate  position  of  p^gg 
using  simple  trigonometry.  Since  the  distance  between  the  vehicle’s  position  and  the 
reference  path  is  the  shortest  possible  distance,  the  orientation  of  the  line  segment  which 
connect  the  two  points  must  be  perpendicular  to  the  orientation  of  the  reference  path,  as 
shown  in  Figure  13.  The  coordinates  of  Pimage  can  fhen  be  calculated  by  adding  the  .v,  and 
v  components  of  dciose  to  the  vehicle’s  coordinate  position.  Therefore,  the  coordinates  of 

Pimage  are* 


27 


(4.18) 


P,mogeX  =  PiX  +  4  WCOS  (P/°  "  K/2)  > 

=  /?/-v  +  rftWsin  O>,-0  -  */2)  > 

2.  Circles 

Finding  pimagC  for  the  case  of  pref  being  a  circle  is  somewhat  trickier  than  the  line 
case,  but  mathematically  the  calculations  are  straightforward.  We  shall  be  able  to 
distinguish  between  the  two  cases  by  examining  pref  K  value.  If  the  value  is  non-zero  than 
the  desired  path  is  a  circle.  Otherwise,  the  path  is  a  line.  A  circle  will  be  defined  by  an  x, 
and  v  point  which  lies  on  the  circle,  with  this  point’s  specific  orientation  and  curvature.  The 
curvature  of  a  circle  is  defined  to  be  a  constant  real  number.  Therefore,  each  point  on  the 
circle  will  have  the  same  curvature  value  as  pref  curvature. 

Pimage*  =  Pref*  ^19> 

The  first  step  in  calculating  the  other  components  of  the  image  position’s 
quadruple  is  to  calculate  the  origin  of  the  reference  circle.  To  do  so  we  calculate  the  radius 
of  the  circle,  which  is  defined  as  the  inverse  of  the  curvature. 

radius  =  — - —  (4.20) 

PrefK 

The  orientation  of  a  point  on  a  circle  is  perpendicular  to  the  orientation  from  the  origin  to 
the  point  on  the  circle,  as  shown  in  Figures  1 1  and  12.  Knowing  the  orientation  of  the  point 
defined  as  pref,  thus  allows  us  to  calculate  the  orientation  from  the  reference  path  to  the 
origin.  Now,  by  dividing  the  radius  into  its’  x,  and  y  components  along  this  orientation  we 
will  find  the  coordinates  of  the  origin.  The  origin  shall  be  referred  to  as  porigin. 

P orieinX  =  Pref  +  radius  <  COS  ^reft  ~  */V>  ) 

(4.21) 

P 'origin-'  =  Pref  +  radius  < sin  (Prcfi  ~  */2>  > 

Which  can  be  written  as, 

P origin  ~  (PrcfX  +  radius  ( sin  {prefi) )  -prefy  +  radius  ( cos  iprefi) )  >  (4.22) 
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The  next  step  in  calculating  the  coordinate  position  of  Pimagc  is  to  calculate  the 
orientation  between  the  origin  and  the  initial  position,  which  we  shall  refer  to  as  y.  This 
orientation  can  be  found  using  the  inverse-tangent  function.  Thus,  y  is  calculated 


(4.23) 


fPj)  Pori gin-  ^ 

y  =  at  an  - - —  (4.23 ) 

\P i*  P origin*  J 

Unfortunately,  this  method  has  two  shortcomings.  We  want  to  distinguish  between  pairs  of 
points  which  may  have  the  same  inverse-tangent  value  but  different  orientations,  one  such 
pair  is  -3n/4  and  it/ 4.  A  second  problem  is  that  equation  (4.24)  is  undefined  for  all  pairs  of 
points  in  which  pjX  =  p0riginx-  However,  we  can  solve  these  problems  by  introducing  a 


variation  of  the  normal  inverse -tangent  function  [Ref.  2]. 

y  =  atan2  (p.y  -  pori.iny-ptx  -  Poriginx) 


(4.24) 


In  equation  (4.25)  we  employ  an  inverse-tangent  function  of  two  arguments.  This  function 
has  a  range  between  [it,  -n].  Therefore,  it  distinguishes  between  equivalent  tangent  values 
of  different  quadrants,  and  is  defined  for  all  values  of  .v. 

With  the  orientation  between  p;  and  porjg„,  we  can  now  calculate  the  coordinates 

of  Pimage  for  a  circle.  The  image  position  is  simply  the  distance  of  the  radius  from  the  origin 
in  the  direction  of  y.  Therefore,  pjB)aec  becomes 


Pimagc X  =  Pori  gin*  +  \radius\  cos  (y) 
Pimagcy  =  Pori  gin?  +  Radius]  sin  (y) 


(4.25) 


The  final  value  we  need  to  calculate  for  Pimage  is  the  orientation  at  the  image 
point.  Once  again  we  shall  use  the  fact  that  any  point  on  a  circle  is  perpendicular  to  the 
orientation  between  the  origin  and  the  point.  Thus,  we  can  calculate  the  image's  orientation 
by  adding  or  subtracting  k/2  to  the  orientation  between  the  origin  and  the  initial  position. 
If  the  reference  path's  curvature  is  negative  we  subtract,  while  if  it  is  positive  we  add.  We 
can  take  advantage  of  the  absolute  value  function  to  incorporate  both  cases  into  one 
equation.  Thus,  the  orientation  of  the  image  point  is 
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-  T+  <*/2>  (££ 


(4.26) 


We  have  successfully  collected  all  the  data  needed  to  find  Pimage-  With  this  point 
established  we  can  then  go  onto  creating  an  algorithm  which  takes  advantage  of  the 
curvature  control  equation  developed  in  chapter  III. 


C.  EVALUATE  NEW  CONFIGURATION 

We  have  now  calculated  all  the  data  we  need  to  assemble  a  working  path  tracking 
algorithm.  After  a  user  inputs  the  vehicle’s  configuration  and  the  desired  reference  path's 
configuration,  our  system  would  calculate  pimage,  then  calculate  the  closest  distance 
between  the  vehicle  and  the  reference  path  by  our  derived  dciosc  equation,  and  finally 
calculate  the  necessary  change  in  curvature  needed  to  move  the  vehicle  towards  and  onto 
the  reference  path.  This  process  would  be  completed  at  predetermined  intervals,  allowing 
foi  a  means  to  update  the  vehicle's  positional  configuration.  However,  before  we  could 
update  the  vehicle’s  configuration  we  would  have  to  calculate  two  values,  the  distance 
traveled  by  the  vehicle  each  interval,  6 disr,  and  the  change  in  the  vehicle’s  orientation  each 
interval  due  to  the  vehicle’s  instantaneous  curvature,  68.  These  values  are  calculated  as 
follows, 

69  =  6 dist  (PjK) 

if  69  =  0  then  bdist  =  6tx\) 

69  (4-27) 

2  cos  — 

if  69*0  then  bdist  =  (5xxv) — 

Where  6t  is  the  duration  of  the  interval,  and  u  is  the  vehicle’s  constant  velocity.  The 
difference  in  the  calculations  for  6 dist  is  a  correction  factor  which  is  intended  to  correct  the 
vehicle's  coordinate  position  when  it  has  a  non-zero  curvature.  By  calculating  these  values 
we  can  update  the  vehicle’s  positional  configuration 
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(4.28) 


pjK=pjK  + 


dK 

ds 


normipfi  +  Sdist  (p(tc) ) 


PjX  =  Pj.x  +  5dist(cos  (pfi  +  50) ) 
Pj v  =  Pj\  +  bdisr  ( sin  (/?(.0  +  60) ) 


The  algorithm  would  continuously  update  the  vehicle’s  position,  and  calculate  the  needed 
change  in  curvature.  This  process  would  effectively  smoothly  merge  the  vehicle  onto  the 
path  and  maintain  it  on  the  path  after  merging. 

D.  SUMMARY 

In  this  chapter  we  developed  the  necessary  equation  to  support  an  algorithm  based  on 
the  curvature  control  equation  developed  in  chapter  ID.  This  basically  consisted  of  the 
development  of  the  image  point  on  the  reference  path,  and  a  consistent  method  to  calculate 
distance.  With  this  information  we  can  establish  a  system  which  will  continuously  calculate 
the  necessary  change  in  the  instantaneous  curvature,  and  update  the  vehicle's  position.  This 
will  effectively  merge  the  vehicle  onto  the  desired  reference  path.  With  this  accomplished 
we  are  ready  to  implement  the  algorithm  into  a  vehicle  simulator  to  test  the  results,  and 
verify1  performance.  With  satisfactory  testing  accomplished  we  could  then  begin  to  work  on 
a  scheme  to  transition  between  multiple  path. 
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V.  TRANSITIONING 


Our  path  tracking  algorithm  successfully  merged  a  vehicle  onto  a  reference  path, 
therefore  the  next  logical  step  in  developing  our  path  tracking  algorithm  is  to  expand  the 
algorithm  to  be  able  to  handle  several  successive  paths.  In  transitioning  between  paths  we 
must  insist  that  the  vehicle’s  motion  is  restricted  within  the  boundary  formed  by  the 
intersecting  paths.  This  requirement  is  to  ensure  vehicle  safety  within  an  unknown 
environment.  Since  our  algorithm  was  designed  to  work  for  all  possible  planar  lines  and 
circles,  the  problem  of  executing  multiple  paths  in  succession  is  reduced  to  the  transitioning 
method  between  successive  paths.  The  primary  question  to  answer  is,  when  do  we  begin 
our  transition  from  one  path  onto  the  next? 

A.  POSSIBLE  METHODS 

Since  we  have  limited  our  research  to  a  spatial  problem,  we  have  limited  the  factors 
which  effect  our  transition  time.  Our  problem  is  not  concerned  with  time,  speed,  and  other 
factors  which  usually  have  a  bearing  on  the  moment  which  actions  are  to  occur.  Rather,  our 
problem  is  solely  dependent  on  distance.  Thus,  we  have  reduced  the  question  of  when  to 
transition,  to  at  what  distance  from  the  next  reference  path  do  we  begin  to  transition.  There 
are  many  different  options  available,  however  we  want  to  limit  the  complexity  while 
maximizing  the  effectiveness  and  fulfilling  our  safety  requirements.  Before  actually 
detailing  our  transitioning  scheme,  let’s  look  at  a  few  possible  options. 

1.  Minimum  Distance 

The  simplest  scheme  can  be  devised  to  transition  when  the  vehicle  is  within  a 
given  distance  from  the  next  reference  path.  This  distance  can  be  either  the  shortest  distance 
between  the  vehicle  and  reference  path,  or  the  distance  between  the  vehicle  and  the 
intersection  of  the  two  reference  paths.  The  option  chosen  is  very  significant.  When  the 


vehicle  reaches  the  determined  minimum  distance,  we  simply  begin  using  the  next 
reference  path  in  the  path  tracking  process.  This  scheme  is  simple,  and  will  satisfactorily 
woik  in  many  cases.  However,  the  minimum  distance  scheme  has  a  serious  flaw. 

The  minimum  distance  scheme  begins  its’  transition  the  same  distance  from  the 
next  reference  path  for  every  situation.  This  scenario  works  well  when  the  desired  turn 
angle  approaches  90  degrees.  However,  problems  arise  when  the  interior  angle  between  the 
present  reference  path  and  the  next  reference  path  deviate  greatly  from  90  degrees.  When 
the  turn  angle  is  large  the  vehicle  often  does  not  have  sufficient  space  available  to  transition 
without  crossing  through  the  new  reference  path.  An  example  of  this  can  be  seen  in  Figure 
14.  where  a  vehicle  attempts  a  170  degree  turn  with  a  minimum  transitioning  distance  of 
three.  Although,  this  distance  was  sufficient  for  a  90  degree  turn,  figure  14  shows  that  the 
vehicle  significantly  overshoots  the  desired  reference  path.  This  wastes  energy  and  time, 
and  could  represent  a  very  serious  hazard  in  an  unknown  environment. 

Alternatively,  when  the  incident  turn  angle  is  small,  the  moment  of  transition  is 
often  earlier  than  desired.  This  is  due  to  when  we  execute  the  transition,  the  image  is  often 
located  a  significant  distance  before  the  intersection  point  of  the  two  successive  paths.  In 
many  cases  this  will  cause  the  vehicle  to  cross  the  previous  reference  path  during  its' 
convergence  to  the  new  reference  path.  In  extreme  cases  the  vehicle  may  even  merge  onto 
the  new  reference  path  prior  to  the  intersection  point  of  the  paths.  This  scenario  is  depicted 
in  figure  1?.  where  the  vehicle  attempts  a  ten  degree  turn  with  a  minimum  transitioning 
distance  of  three.  Once  again  this  deviation  from  the  desired  motion  wastes  energy  and 
time,  and  represents  a  danger  to  the  vehicle.  Therefore,  the  minimum  distance  scheme  must 
be  considered  unsuitable  for  our  system. 

2.  Dynamic  Transitioning 

A  second  transitioning  option  would  be  to  vary  the  transitioning  distance 
according  to  the  path  tracking  problem.  The  system  would  dynamically  calculate  the 
optimum  distance  to  make  a  smooth  efficient  turn.  To  accomplish  this  method  we  would 


establish  a  turn  angle  as  a  reference  point  on  which  to  vary  the  transitioning  distance.  This 
benclunaik  would  be  a  90  degree  turn,  with  a  transitioning  distance  of  two  times  the 
distance  constant.  Sy.  The  dynamic  scheme  would  increase  this  distance  for  tighter  turns, 
and  decrease  the  distance  for  wider  turns.  We  could  further  improve  this  scheme  by 
incorporating  other  factors  which  affect  the  vehicle’s  maneuverability,  e.g.  curvature 
limitations,  velocity,  distance  constant,  and  the  vehicle’s  maneuverability  characteristics. 

The  dynamic  transitioning  described  would  be  extremely  effective  in  optimizing 
the  transitions  between  paths  It  would  maintain  the  vehicle’s  movement  to  within  the 
perimeter  outlined  by  the  reference  path.  This  would  increase  vehicle  efficiency, 
maneuverability,  and  control,  which  would  thereby  enhance  safety.  However,  this  type  of 
system  has  some  disadvantages.  The  system  would  require  a  complex  function  or  a  large 
data  table  to  properly  determine  the  correct  transitioning  distance.  Either  method  would  be 
costly  in  time  and  hardware  support.  Furthermore,  both  options  would  require  exhaustive 
experimentation  to  derive  the  necessary  data.  This  would  be  time  consuming  and  dreary  for 
a  computer  science  student.  Therefore,  we  feel  although  this  method  yields  excellent 
results,  it  is  not  appropriate  for  our  system  at  this  time. 

B.  A  SUITABLE  SCHEME 

The  transitioning  schemes  we  introduced  wore  unsatisfactoiy  for  our  system.  The 
minimum  distance  scheme  did  not  fulfill  our  requirements,  while  the  dynamic  scheme  was 
too  costly.  Therefore,  the  scheme  which  we  settled  on  was  a  simplified  variation  of  the 
dynamic  transitioning  scheme.  Our  variation  will  have  a  transitioning  distance  function 
based  on  the  turn  angle  and  the  distance  constant.  The  transitioning  distance  will  refer  to 
the  distance  from  the  vehicle  to  the  intersection  point  of  the  paths.  We  will  conduct 
transitioning  experiments  to  collect  the  necessary  data  concerning  the  relationship  between 
these  factors.  Given  the  appropriate  data  we  shall  extrapolate  a  general  function  which 
meets  our  requirements.  The  idea  is  to  derive  a  function  which  may  not  provide  the 
optimum  transitionmg  distance,  but  will  provide  an  appropriate  distance  in  all  cases. 
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1.  Intersection  Point 


Our  transitioning  distance  shall  be  measured  from  the  vehicle  to  the  intersection 
point  of  the  successive  paths.  Theiefore,  we  must  have  a  procedure  which  locates  the 
intersection  point  for  all  possible  transition.  There  are  four  tran'  .on  possibilities;  line  to 
line,  line  to  circle,  circle  to  line,  and  circle  to  circle.  The  use  r  os  in  our  path  tracking 
algorithm  is  for  obstacle  avoidance.  Therefore,  it  would  v>.  inappropriate  to  transition 
between  two  citcles.  This  being  the  case  we  shall  exclu  te  the  circle  to  circle  case  in 
deriving  the  intersection  point  for  our  research. 

a.  Line  to  Line 

The  normal  line  to  line  intersection  problem  is  very  simple  when  we  have  the 
lines  in  the  slope  intersect  format.  However,  our  lines  are  in  the  configuration  format.  The 
method  we  shall  use  to  calculate  the  intersection  point  is  based  on  the  Law  of  Sines.  To 
calculate  the  point  of  intersection  we  are  going  to  construct  a  triangle  from  the  two 
reference  paths,  calculate  the  distance  of  one  of  the  sides  of  the  triangle,  and  calculate  the 
interior  angles  of  the  triangle.  With  this  information  we  can  calculate  any  of  the  side’s 
distances,  and  the  coordinates  of  the  intersection  point.  A  graphic  description  of  a  line 
intersection  problem  can  be  seen  in  Figure  16.  We  shall  now  calculate  S,  the  distance 
between  the  two  reference  configurations.  This  can  be  accomplished  using  the  Euler 
distance  equation, 

5  =  J(P,e/2X-PreflX')2+  (Pre/2y-Prefiy')2 
We  then  calculate  the  orientation  between  these  two  configurations,  T,  using 
the  inverse-tangent  function  described  in  equation  (4.25). 

T  =  atm2{piefty-pref\y,pref2.x-prefl.x)  (5.2) 

With  this  orientation  we  can  construct  a  triangle  from  the  two  directed  lines,  by  projecting 
a  line  segment  in  the  orientation  of  T  between  the  two  reference  paths.  We  now  have  the 
necessary  information  to  calculate  the  interior  angles,  as  shown  in  Figure  17. 
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P  =  |/V,e-r] 

a  =  \nonn{K-T)  -prep$\ 
<t>  =  (7i- p- a) 


(5.3) 


With  the  interior  angles  calculated  we  can  use  the  Law  of  Sines  to  calculate 
the  distance  of  one  of  the  other  two  sides  of  the  triangle.  We  will  calculate  Sj.  the  distance 
from  the  intersection  point  and  the  first  reference  path. 

sin  (a) 

s>  ■  <“> 

This  provides  us  with  the  needed  information  to  calculate  the  intersection  point.  The 
coordinates  of  the  intersection  are  calculated  by  dividing  Sj  into  its  x,  and  y  components, 
and  adding  to  the  coordinates  of  the  reference  path  configuration 

Pinterx  =  PreflX  +  SlCOS(Pref\fy 

Pin,ery=P,ef\y  +  Slsin(P,eflfy  (5 -7) 

P  inter*  =Prefl%-Prefl* 

The  orientation  of  Pinter  is  simply  equal  to  the  interior  angle  fonned  by  the 

intersecting  reference  paths.  This  value  is  being  calculated  for  future  use  in  determining  the 
transition  distance  required.  It  will  be  referred  to  in  the  future  as  the  turn  angle. 


b.  Line  to  Circle/  Circle  to  Line 

The  transition  between  line  to  circle  and  circle  to  line  are  very  similar.  We 
assume  the  user  inputs  two  path  which  intersect.  Given  this  assumption,  a  line  will  intersect 
with  a  circle  in  either  one  or  two  points.  In  either  case  both  scenarios  reflect  both  transition 
possibilities  depending  on  which  point  of  intersection  we  choose.  This  example  is 
illustrated  in  Figure  18.  The  first  step  in  determining  the  intersection  point  is  to  calculate 
the  origin  of  the  circular  reference  path.  We  calculate  the  radius,  and  then  use  equation  (4.6) 
to  find  the  origin’s  coordinates. 
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radius  = 


1 


PrefK 

Pongin'  =  Prof  ~  radiuS  <  sbl  (P ref 0) )  ^ 

Pongin''  =  PrtfV  +  radius  (  cos  (Prtf6)  > 

The  trick  now  is  to  create  a  triangle  with  porigin  and  the  directed  line  in  order  to  find  the 
intersection  point.  After  creating  this  triangle  we  can  use  the  properties  of  a  triangle  to 
calculate  the  measurements  of  the  sides,  and  to  then  find  the  coordinates  of  the  intersection 
point,  as  shown  in  Figure  18.  To  begin  let’s  calculate  the  distance  from  the  origin  to  the 
directed  line.  A.  using  equation  (4.18). 

(5-9) 


A  =  ( Po,igh,y-Prefy )  (COS  (Preft))~  (Pongin' -Pongin')  (S  m(preft)) 
Using  the  Pythagorean  theorem  we  can  calculate  the  distance  B.  If  I  A  I  >  I  radius  I,  then 
there  exists  no  intersection  point  between  the  two  paths. 


B  =  (  — )  -A2 

V  Prep 


(5.10) 


We  can  calculate  the  image  point  of  the  oiigin  onto  the  reference  path  by 
using  the  value  of  A,  and  the  fact  that  this  distance  is  perpendicular  from  the  origin  with 
respect  to  the  directed  line.  The  image  is 


P image'  ~  P origin'  A  s^n  (P reft) 

P imaged  ~  P origin -  ~  A  C0S  (P reft) 

Witli  the  image  calculated  we  can  now  find  the  intersection  point. 

P inter'  =  Pimagc'±Bcos  ( Preft > 

P inter'  ~  P image-’  ~  B S7n  ( Preft) 


(5.11) 


(5.12) 


This  equation  is  suitable  for  both  line  to  circle  and  circle  to  line  transitions.  If 


we  are  transferring  from  a  line  to  a  circle  we  would  subtract  the  components  of  B  from  the 
image,  and  if  we  go  from  a  circle  to  line  we  add  the  components.  To  calculate  the 


orientation  of  the  intersection  point  we  once  again  use  our  variation  of  the  invetse-tangent 
function. 

<I>  =  ata n2{pittlery-porigi^interX-porigi^)  (5.13) 

This  orientation  is  perpendicular  to  the  orientation  of  the  intersection  point,  which  can  be 
calculated  by  adding  or  subtracting  Jt/2  radians,  depending  on  the  sign  of  the  curvature  of 
the  circular  reference  path. 


p. . 0  =  <t>  + 


inter 


PrefK 
I  P.efK 


(5.14) 


Tlius,  we  have  successfully  calculate  the  intersection  point  for  all  three 
transition  possibilities.  Using  this  point  we  shall  be  able  to  calculate  the  distance  from  the 
intersection  to  control  the  time  of  transition.  Now,  we  are  ready  to  proceed  to  detennining 
the  transitioning  distance  function. 


2.  Transitioning  Distance 

Our  transitioning  scheme  is  based  on  transferring  between  successive  paths  at  a 
time  detennined  by  the  distance  from  the  vehicle  to  the  next  path.  This  distance  shall  be 
referred  to  as  the  transitioning  distance.  To  optimize  the  efficiency  of  transitioning  the 
transitioning  distance  can  not  be  a  constant  value.  Therefore,  it  is  our  idea  to  find  a  function 
that  will  determine  the  appropriate  distance  between  the  vehicle  and  path  to  produce  an 
efficient  transfer.  However,  it  is  highly  unlikely  that  a  general  function  will  produce  the 
optimum  transitioning  distance  for  all  possible  situations.  Thus,  our  objective  is  to  derive  a 
function  which  will  ensure  all  transfers  between  paths  meet  our  requirements  for  safety  in 
an  efficient  manner.  The  safety  requirements  are  based  on  maintaining  positive  control  over 
the  vehicle  to  the  extent  that  the  vehicle  avoid  oscillation,  and  does  not  cross  the  bounds 
established  by  the  reference  paths.  Our  method  of  deriving  the  function  will  be  based  on 
gathering  experimental  data,  which  will  translate  into  an  appropriate  function. 

To  derive  a  transitioning  function  we  intend  to  conduct  experimental  tests  to 
determine  the  critical  distance  necessary  for  a  safe  turn.  A  safe  turn  is  defined  to  be  a  turn 
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in  which  the  vehicle  negotiates  the  intended  turn  without  crossing  the  present  reference 
path  or  overshooting  the  next  successive  path.  The  experimental  tests  will  maintain  the  turn 
angle  and  the  distance  constant,  while  varying  the  available  distance  to  maneuver  the 
vehicle.  This  process  will  be  repeated  until  we  find  die  minimum  distance  needed  to  fulfill 
the  requirements.  The  range  of  the  experiment  will  consist  of  turn  angles  between  [0,  n]  at 
intervals  of  7t/12.  We  shall  also  complete  the  experiments  for  various  values  of  Sq.  The 
results  of  our  experiments  will  be  put  into  a  table  format  to  enable  quick  comparison  of 
results.  We  shall  then  develope  a  distance  function  which  will  approximate  the  results  of 
our  experimental  data. 

In  conducting  these  experiments  the  major  concern  is  determining  if  the  generated 
path  oscillates  or  crosses  the  reference  path.  To  accomplish  this  we  shall  manipulate  the 
vehicle’s  initial  position  to  simulate  the  desired  turn  and  transitioning  distance.  To  illustrate 
this,  we  refer  to  Figure  19. 


Pi  =(x,y) 


Figure  19.  Rotation  of  a  Transition  Problem 
The  problems  displayed  in  both  figures  are  identical  and  will  be  handled  by  the  simulator 
in  the  same  manner.  We  are  simply  changing  the  vehicle's  initial  configuration  to  simulate 
our  desired  transition.  Thus,  we  convert  every  transitioning  situation  into  a  transition  to  the 
x-axis.  In  this  case  we  can  quickly  determine  if  the  path  oscillates  or  crosses  the  reference 
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path  by  simply  checking  the  sign  of  the  vehicle’s  position.  If  the  value  of  the  vehicle's  v 
component  ever  goes  negative,  then  the  vehicle  has  crossed  the  reference  path,  and  the 
transitioning  distance  is  insufficient  for  that  scenario.  Thus,  it  is  our  desire  to  translate  all 
experimental  transitioning  into  transitions  onto  the  x-axis. 

The  graphical  results  of  many  of  the  experiments  can  be  viewed  in  appendix  B. 
Table  1  below  shows  the  experimental  data  for  the  simulations.  Thus,  the  table  is  a  concise 
means  to  display  the  relationship  between  the  turn  angle,  distance  constant,  and 
transitioning  distance.  The  rows  represent  different  distance  constants  while  the  columns 
are  different  turn  angle.  The  entry  under  any  specific  row  /column  is  the  required  minimum 
transitioning  distance  for  that  particular  problem.  For  example,  the  minimum  transitioning 
distance  for  a  90  degree  turn  with  a  distance  constant  of  0.5  is  1.3units,  depending  on  the 
units  of  Sq. 

3.  Deriving  A  Transitioning  Function 

Given  this  experimental  data  we  can  now  determine  a  general  function  for  the 
transitioning  distance.  Examining  the  entries  of  the  Table  1.  we  quickly  come  to  the 
conclusion,  that  there  exists  no  simple  function  to  determine  the  transitioning  distance  for 
all  different  situations.  Therefore,  let's  attack  the  problem  by  dividing  the  problem  into  two 
parts,  the  relationship  between  the  turn  angle  and  the  transitioning  distance,  and  the 
relationship  between  the  distance  constant  and  the  transitioning  distance.  By  solving  these 
two  simpler  problems  and  then  combining  the  results,  we  can  derive  a  composite  function 
which  is  suitable  to  fulfill  our  requirements. 

a.  Relationship  Between  TD  and  Turn  Angle 

First  let  us  examine  the  relationship  between  the  turn  angle  and  the 
transitioning  distance.  It  is  an  obvious  observation  that  it  takes  a  greater  transitioning 
distance  to  complete  a  sharper  turn  than  a  wider  turn.  To  find  the  underlying  relationship 
between  the  two  values  we  can  plot  the  entries  of  the  Table  1  for  a  given  value  of  the 


44 


distance  constant.  We  can  then  use  this  plot  to  extrapolate  a  distance  function  from  the 
graph.  The  example  we  plotted  was  the  case  of  Sq  =  0.25,  and  is  illustrated  in  Figure  20. 
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Figure  20.  Experimental  Minimum  Transition  Distance 

We  plotted  the  tum  angle  along  the  x-axis  from  -165  to  165  degrees.  Since 
the  negative  tum  angles  are  turns  of  the  same  magnitude  but  in  the  opposite  direction  as  the 
positive  turns,  they  require  the  same  transitioning  distance  as  their  positive  counterparts. 
On  the  y-axis  is  plotted  the  minimum  required  distance  to  complete  a  given  tum  angle.  The 
results  of  our  efforts  clearly  reveal  a  relationship  between  these  two  variables.  The  shape 
of  the  graph  is  similar  to  that  of  a  parabola,  however  the  base  of  the  curve  is  much  flatter. 
What  we  desire  is  to  find  a  function  which  will  map  to  this  plot  as  close  as  possible,  but 
never  produce  a  transitioning  distance  smaller  than  that  produced  by  our  experimental  data. 
Thus,  let's  assume  the  relationship  is  in  the  form  of  a  fourth  order  polynomial,  which  share 
the  basic  shape  of  the  parabola. 


TD(W  = 


1 


$  4 


(5.15) 
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Table  1:  EXPERIMENTAL  TRANSITIONING  DISTANCE 


Turn  Angle 
(Degrees) 

Distance  Constant 

s0=i.o 

Sy=0.5 

Sq=0.25 

S0=0. 1 25 

0 

0.0 

0.0 

0.0 

0.0 

15 

2.0 

1.1 

0.6 

0.3 

30 

2.1 

1.1 

0.7 

0.3 

45 

2.1 

1.2 

0.7 

0.4 

60 

2.1 

1.2 

0.8 

0.4 

75 

2.2 

1.2 

0.9 

0.5 

90 

2.3 

1.3 

0.9 

0.5 

105 

2.5 

1.4 

1.0 

0.5 

120 

2.9 

1.6 

1.1 

0.6 

135 

3.5 

1.9 

1.3 

0.7 

150 

4.8 

2.6 

1.7 

1.0 

165 

8.8 

4^ 

bo 

3.0 

1.9 

180 

— • 

i 

i 

i 

i 

| 
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Where.  <j>  is  the  turn  angle  in  radians  and  TD  is  the  transitioning  distance.  By  converting  the 
turn  angle  into  a  ratio  between  the  turn  angle  and  the  maximum  possible  turn,  we  simulate 
the  curve  of  Figure  20.  Equation  (5.15)  will  produce  a  flat  plot  when  the  turn  ratio  is  close 
to  zero,  while  exponentially  increasing  the  transitioning  distance  as  the  turn  ratio 
approaches  positive  or  negative  unity. 

If  we  calculate  the  transitioning  distance  by  this  function  for  those  points  in 
which  we  collected  experimental  data,  we  can  determine  the  appropriateness  of  the 
function.  We  set  up  a  plot  similar  to  that  of  Figure  20  to  display  the  results  of  equation 
(5.15).  However,  we  included  all  turn  angles  between  -170  and  170  degrees.  Figure  21 
displays  the  results.  The  plot  illustrates  a  very  similar  curve  to  that  of  the  experimental  data. 
A  quick  check  of  the  individual  values  also  reveals  that  the  function  produces  results  which 
at  all  times  is  greater  than  those  of  the  experimental  data  for  the  case  of  So  equal  to  0.25. 
Therefore,  this  function  is  an  appropriate  approximation  for  determining  transitioning 
distance.  However,  further  comparisons  between  the  output  of  this  function  and  entries  in 
Table  1  clearly  illustrates  that  this  function  does  not  fulfill  our  needs.  The  problem  is  that 
the  function  does  not  take  into  account  the  effect  altering  the  value  of  the  distance  constant 
has  on  the  minimum  transitioning  distance. 

b.  Relationship  Between  TD  and  Sq 

Equation  (5.15)  is  unsuitable  for  non  constant  Sq.  However,  this  equation 
does  provide  us  with  a  base  case  transitioning  function.  That  is  we  can  use  this  function  to 
determine  the  transitioning  constant,  but  modify-  its’  output  as  appropriate  dependent  on  the 
actual  value  of  the  distance  constant.  Thus,  we  are  basically  creating  a  composite  function 
of  a  function  dependent  on  the  turn  angle  and  a  function  dependent  on  the  distance  constant. 
To  derive  the  function  dependent  on  the  distant  constant  we  shall  follow  the  same  method 
we  used  with  the  turn  angle.  Therefore,  once  again  let’s  plot  the  experimental  results. 
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TRANSITION  DISTANCE 


TURN  ANGLE  (degrees') 

Figure  21.  TD  as  Calculated  by  Table  1  and  Equation  (5.15) 
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This  would  produce  12  independent  curves,  one  for  each  turn  angle,  which  illustrate  the 
relationship  between  the  value  of  S0  and  TD.  The  case  for  turn  angles  of  150  and  15  degrees 
are  displayed  in  Figure  22. 
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DISTANCE  CONSTANT  (cm.) 

Figure  22.  Relationship  Between  TD  and  S0 

This  figures  show  that  the  relationship  between  the  value  of  Sq  and  TD  is  linear.  In  fact, 
each  of  the  twelve  plots  indicate  that  this  relationship  is  linear.  Therefore,  it  may  be 
possible  to  extrapolate  a  linear  equation  from  these  plots  which  will  provide  a  correction 
factor  for  different  distance  constants. 

If  we  begin  looking  for  a  relationship  between  the  12  individual  plots,  we 
quickly  realize  that  the  slope  of  the  twelve  plots  vary  significantly.  This  would  make  it  very 
difficult  to  find  a  linear  equation  which  would  satisfy  all  possible  cases.  However,  since  our 
transitioning  distance  function  (5.15)  is  based  on  the  case  of  Sq  =  0.25,  we  can  standardize 


the  entries  of  Table  1.  By  plotting  the  standardized  results  we  can  more  effectively  isolate 
the  effect  the  value  of  the  distance  constant  has  on  the  transitioning  distance.  Thus,  Table 
2  is  an  abridged  version  of  Table  1  with  the  entries  converted  into  ratios.  The  row/  column 
entries  of  Table  2  are  the  ratios  between  the  transitioning  distance  for  a  specific  tum  angle 
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and  S0,  and  the  standard  transitioning  distance  for  that  turn  with  Sq  =  0.25.  The  standard 
transitioning  distance  is  the  value  outputted  by  equation  (5.15)  for  a  specific  turn  angle. 


Table  2:  TRANSITION  DISTANCE  RATIO 


Turn  Angle 
(Degrees) 

Standard 

TD 

Distance  Constant  Ratio 

4.0 

2.0 

1.0 

0.5 

15 

1.000 

2.0 

1.1 

0.6 

0.3 

90 

1.067 

2.16 

1.22 

0.84 

0.47 

165 

3.402 

2.56 

1.41 

0.88 

0.56 

Now,  if  we  plot  the  results  of  Table  2,  we  can  find  an  appropriate  correction 
factor.  Figure  23  depicts  the  relationship  between  the  ratios  of  transitioning  distance  and 
turn  angles. 

3  — - - - -  - - - - . 


DISTANCE  CONSTANT  (So  C  25) 

Figure  23.  Plot  of  the  Ratios  of  TD  and  S0 

In  Figure  23  we  plotted  all  three  turn  angles  included  in  Table  2.  As  we  can  see  in  each  case 
the  resultant  curve  is  very  similar,  with  nearly  the  same  slope.  Thus,  this  indicates  that  we 
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can  derive  a  linear  correction  factor  for  various  distance  constants.  Using  the  slope  intercept 
line  equation  we  get. 

CF{S0}  =  (  4.o~-1.0")a  +b  {5A6) 

CF(S0)  =  0.56.V  +  b  (5.17) 

Where  CF  is  the  correction  factor,  x  is  tire  distance  constant  standardized  with  respect  to 
the  value  of  0.25.  and  b  is  the  y-intercept.  Using  equation  (5.17)  and  the  point  (1 , 0.88)  from 
our  plot  we  can  solve  for  b.  The  value  of  b  is  0.32,  and  equation  (5.17)  becomes, 

CF(S0)  =  (0.56)  +0.32  (5.18) 

This  equation  was  a  suitable  correction  factor  for  various  distance  constant 
values,  however  we  desire  to  match  the  experimental  data  as  close  as  possible.  Therefore, 
we  adjusted  this  linear  equation  to  pioduce  results  which  «'ould  match  the  results  of  the 
transitioning  function  with  those  of  (he  experimental  data.  We  accomplished  this  simply  by 
analyzing  the  twelve  cases  and  interpolating  appropriate  values  for  the  slope  and  intercept. 
The  final  results  were, 

St) 

CF  (S0)  =  0.6(~^)+0.3  (5.19) 

By  combining  the  results  of  both  relationships  we  can  create  our  composite 
function.  Thus,  our  function  for  determining  the  transitioning  distance  for  all  turn  angle  and 
distance  constants  is 


TD{^{))  =  [2.45u  +  0.3] 


<t> 

n 


(5.20) 


This  equation  is  simply,  but  robust  enough  to  apply  in  all  possible  transition  problems.  To 
verify  the  effectiveness  of  this  equation,  we  shall  compare  the  results  of  the  equation  to  the 
experimental  results.  Table  3  gives  us  the  transitioning  distance  results  by  using  the  derived 


Table  3:  CALCULATED  TRANSITIONING  DISTANCE 


Turn  Angle 
(Degrees) 

Distance  Constant 

o 

T— < 

II 

» 

00 

S0=0.5 

S0=0.25 

S0=O.125 

0 

0.0 

0.0 

0.0 

0.0 

15 

2.70 

1.50 

0.90 

0.60 

30 

2.702 

1.501 

0.90 

0.60 

45 

2.711 

1.505 

0.904 

0  602 

60 

2.734 

1.519 

0.911 

0.608 

75 

2.784 

1.547 

0.928 

0.619 

90 

2.880 

1.60 

0.960 

0.640 

105 

3.054 

1.696 

1.079 

0.679 

120 

3.365 

1.869 

1.121 

0.748 

135 

3.950 

2.194 

1.317 

0.878 

150 

5.215 

2.897 

1.738 

1.159 

165 

9.186 

5.103 

3.062 

2.041 

180 
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function.  As  we  can  see  by  simple  comparison  of  the  entries  of  Table  1  and  Table  3,  in  all 
cases  our  derived  function  outputs  a  value  equal  to  or  greater  than  that  derived  by 
experimentation.  Therefore,  we  can  assume  that  our  derived  equation  is  appropriate,  and 
will  ensure  sufficient  maneuvering  space  while  minimizing  waste. 

C.  SUMMARY 

In  this  chapter  we  introduced  some  possible  transitioning  schemes  for  our  algorithm. 
The  scheme  on  which  we  settled  for  is  a  variation  of  a  dynamic  scheme.  Our  scheme  is 
based  on  transitioning  a  variable  distance  from  the  intersection  point  of  two  successive 
paths.  This  distance  is  determined  by  a  general  function  which  takes  into  account  the  degree 
of  turn  involved  and  the  desired  distance  constant.  The  function  was  derived  by  gathering 
experimental  data  through  use  of  our  simulator.  With  the  results  of  these  experiments  we 
inteipolated  a  general  function,  which  would  fulfill  our  safety  requirements  while 
maximizing  transitioning  efficiency.  Some  graphical  results  are  included  in  figures24  to  26, 
which  illustrate  the  effectiveness  of  our  transitioning  system.  In  Figure  24  we  are 
completing  a  simple  90  degree  turn  with  a  Sq  value  of  1 .0.  Figure  25  combine  the  line  circle 

transitioning  combinations,  and  illustrates  the  obstacle  avoidance  problem.  In  this  graph  we 
can  assume  the  vehicle  isolates  an  obstacle  ahead  on  its’  present  path.  The  vehicle  thus, 
transitions  to  an  appropriate  dimension  circle  to  avoid  the  obstacle.  When  the  vehicle  is 
clear  of  the  obstacle  it  transitions  back  onto  its’  original  path.  The  remaining  figure  displays 
the  transitioning  between  two  circles. 
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Figure  24.  Transition  From  Inner  to  Outer  Circle 
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Figure  25.  Transition  From  Outer  to  Inner  Circle 
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Figure  26.  Transitions  Involved  in  Obstacle  Avoidance 
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VI.  IMPLEMENTATION 


One  of  the  primary  objectives  of  our  research  was  to  provide  greater  flexibility  and 
maneuverability  for  an  autonomous  vehicle.  The  path  tracking  method  which  we  propose 
would  accomplish  this  by  providing  a  simple,  but  powerful  means  to  control  a  vehicle’s 
motion.  However,  to  maximize  the  benefits  of  our  pathtracking  algorithm,  we  should 
incorporate  it  with  a  traditional  path  planning  algorithm,  such  as  that  described  for 
Kanayama's  Yamabico-11  [Ref.  3].  The  resultant  algorithm  will  enjoy  flexibility, 
maneuverability,  ease  of  operation,  and  increased  control.  Therefore,  our  final  objective  is 
to  implement  our  path  tracking  algorithm  into  the  software  system  of  an  autonomous 
vehicle.  Our  system  has  been  designed  with  regards  for  all  categories  of  vehicles. 
Therefore,  our  algorithm  will  be  compatible  for  the  Yamabico-1 1  robot. 

A.  OVERVIEW  OF  SYSTEM 

I.  MMI,  System 

The  Yamabico-Il  robot  presently  operates  within  the  MML  software 
environment.  MML  is  an  abbreviation  for  Model-based  Mobile  robot  Language,  and  is  a 
on-line  library  of  mobile  robot  oriented  functions.  MML  is  a  real  time  motion  control 
system  for  off-line  programming  of  a  mobile  robot.  It  consists  of  three  primary 
components;  a  path  planner,  a  motion  generator,  and  a  tracking  controller  module.  The 
system  accepts  the  user’s  desires  in  the  form  of  a  user  program.  The  path  planning  module 
takes  this  input  and  converts  it  into  a  description  of  the  path.  This  path  description  is  used 
in  conjunction  with  the  velocity  and  acceleration  specifications  to  produce  reference 
configurations  which  track  along  the  desired  path.  This  data  is  compared  by  the  tracking 
controller  module  to  minimize  the  difference  between  the  vehicle’s  position  and  the 
reference  configuration.  The  controller  module  also  consists  of  all  the  necessary 
mechanisms  for  locomotion  capabilities  including  the  vehicle's  motor  and  wheels  [Ref.  3]. 
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B.  PATH  TRACKING  SYSTEM 


The  system  we  envision  shall  he  comparable  to  the  existing  system  There  shall  be  two 
modules  which  parallel  the  path  planner,  and  the  generator.  These  modules  shall  be  referred 
to  as  the  path  descriptor,  and  the  executor.  The  output  of  these  modules  shall  provide  input 
to  the  existing  tracking  controller  module.  The  process  shall  be  initiated  by  the  creation  and 
linking  of  a  user  module  to  the  path  tracking  system.  Our  system  shall  be  written  in  the  C 
language  within  the  Unix  environment. 

1.  User  Program 

The  user  program  shall  be  designed  for  off-line  programming  of  the  robot.  Its 
purpose  is  to  provide  a  means  for  the  user  to  program  the  robot’s  motion.  The  user  module 
shall  consist  of  an  indeterminate  number  of  path  tracking  locomotion  functions,  which 
describe  the  robot's  desired  motion.  The  available  commands  are  described  in  appendix  D. 
The  sequence  of  locomotion  commands  must  be  completed  by  listing  the  execute  command 
after  all  other  commands.  Execute  acts  as  a  signal  to  the  system  to  transfer  control  to  the 
executor  module.  The  user  program  shall  be  executed  under  the  supervision  of  the  MML 
operating  system.  The  path  tracking  functions  within  the  user  module  shall  be  linked  only 
at  compile  time.  This  module  thereby  acts  the  same  as  it  did  for  the  user  program  of  the 
MML  system. 

2.  Path  Descriptor 

The  path  descriptor  module  shall  perfonn  a  similar  function  to  the  path  planner 
module  of  the  MML  system.  !*  shall  take  a  user  program  and  convert  it  into  a  description 
of  the  desired  motion  in  terms  acceptable  to  the  executor  module.  This  shall  be 
accomplished  by  taking  the  sequence  of  commands  of  the  user  program  and  placing  their 
corresponding  path  information  onto  an  instruction  stack.  The  stack  shall  comprise  of  three 
components:  the  class,  the  reference  path,  and  the  transition  point.  An  example  of  a  typical 
instruction  stack  is  given  in  Table  4. 
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Table  4:  INSTRUCTION  STACK 


Class 

Reference  Path 

Intersection 

spath 

(xr5-  y'r5'  0r5>  ^r5  1 

— 

ppath 

(xr4.  Vr4.  0r4.  Kr4) 

<*i3-  yi3-  0i3>  Ki3> 

path 

(xr3'  Yr3'  ®r3’  ^r3^ 

(xi2*  yi2.  0i2>  *i2) 

ppath 

(Xr2.  yr2.  er2.  Kr2) 

(*ii>yii-0ii.Kii> 

path 

(xr).  Vfi-  0r],  Krl) 

— 

The  class  is  the  type  of  path  locomotion  function.  There  are  only  three  possible 
options:  path,  ppath.  and  spath.  The  path  function  is  an  indefinite  move  command  onto  the 
path  described  by  the  given  configuration.  The  function  ppath  is  a  partial  path  function, 
which  is  simply  a  path  with  a  distinct  endpoint  for  transitioning  purposes.  The  path  and  the 
endpoint  are  described  by  the  same  configuration.  The  last  available  class  is  spath,  which 
is  a  stop  path  function.  The  accompanying  reference  configuration  refers  to  the  location  on 
the  path  for  which  the  robot  is  to  stop. 

The  reference  path  and  the  intersection  are  configurations  which  describe  the 
desired  motion.  The  user  program  shall  have  a  path  configuration  for  each  command 
entered  to  delineate  a  specific  path.  This  configuration  shall  be  placed  on  the  stack  as  the 
desired  reference  path.  The  system  shall  then  use  the  class  and  successive  reference  path 
configurations  to  determine  the  intersection  point.  This  intersection  point  shall  be  used  to 
determine  when  to  transition  between  successive  paths. 

3.  Executor 

The  executor  module  will  be  initiated  by  the  user  program.  After  all  locomotion 
functions  have  been  placed  onto  the  instruction  stack  the  command  execute  will  pass 
control  to  the  executor.  The  objective  of  the  executor  is  to  derive  the  dK/ds  necessaiy  to 
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merge  onto  the  reference  path.  This  is  simply  a  call  to  our  path  tracking  algorithm  with  the 
specific  instruction  stack.  After  the  lobot's  image  has  reached  the  determined  transitioning 
point  the  next  entry  of  the  instruction  stack  shall  be  executed.  This  shall  be  continued  until 
all  instructions  have  been  executed.  At  this  point  the  robot  shall  switch  to  the  stop  state. 

C.  SUMMARY 

One  of  the  primary  objectives  of  our  research  was  to  expand  the  motion  control 
capabilities  of  all  categories  of  autonomous  vehicles.  In  accordance  with  this  we  developed 
a  simple  but  powerful  path  tracking  algorithm.  To  show  the  benefits  of  such  an  algorithm 
we  intend  to  implement  it  within  the  operating  system  of  a  mobile  robot,  Yamabico- 1 1 .  The 
implementation  that  we  decided  to  undertake  was  based  on  a  parallel  system  to  the  present 
Yamabico- 1 1  operating  system,  MML.  This  would  allow  us  to  employ  applicable  existing 
modules  of  the  MML  system,  while  developing  other  modules  to  tailor  the  system  to  our 
patli  tracking  algorithm.  Our  ultimate  desire  is  to  create  a  operating  system  which  could  be 
quickly  operational,  and  expanded  to  subsume  the  MML  system  in  the  future. 
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VII.  SUMMARY  AND  CONCLUSIONS 
A.  CONTRIBUTIONS  OF  RESEARCH 

The  benefits  of  our  path  tracking  algorithm  are  primarily  within  two  areas.  The  first 
area  is  concerned  with  the  effective  control  of  an  autonomous  vehicle.  Most  present  motion 
control  algorithms  for  autonomous  vehicles  are  based  on  a  exact  positioning.  That  is.  the 
vehicle  is  controlled  to  pass  through  certain  desired  points.  This  method  is  extremely  useful 
and  versatile.  However,  many  instances  exist  where  the  exact  positioning  of  die  vehicle  is 
not  as  important  as  the  orientation.  In  our  path  tracking  algorithm  we  emphasize  translating 
our  desired  motion  into  simple  terms.  VVe  make  it  considerably  easier  for  the  user  to 
program  a  vehicle’s  motion  by  alleviating  the  need  to  predetermine  points  along  the  motion 
path.  In  addition,  the  general  concept  of  motion  behind  our  path  tracking  algorithm 
provides  a  more  general  approach,  which  is  as  encompassing  as  the  more  prevalent  point 
to  point  approach.  The  path  tracking  approach  allows  the  user  to  maintain  the  vehicle  on 
curves  which  would  be  impossible  to  replicate  in  the  point  to  point  approach.  Both  methods 
offer  the  user  some  advantages,  however  it  is  best  when  the  methods  are  combined  within 
a  vehicle’s  software.  This  greatly  enhances  the  vehicle’s  motion  control  flexibility  and  user 
operability. 

The  second  area  which  this  research  directly  benefits  is  that  of  obstacle  avoidance.  In 
our  path  tracking  scheme  we  included  tracking  along  circular  paths.  The  primary  reason  we 
implemented  a  path  tracking  algorithm  for  circles  is  to  provide  a  means  for  obstacle 
avoidance.  Our  intentions  are,  that  when  a  vehicle’s  sensors  detect  an  obstacle,  we  would 
have  an  automatic  transition  from  the  path  the  vehicle  is  presently  on  to  a  circular  path 
which  would  circumvent  the  obstacle.  After  avoiding  the  obstacle  the  vehicle  would  then 
transition  from  the  circle  back  onto  the  original  path.  A  more  complex  scheme  can  be 
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developed  on  this  idea  to  handle  multiple  obstacles  and  dynamic  moving  obstacles. 
However,  the  path  tracking  scheme  presents  the  foundation  for  these  ideas. 

B.  FUTURE  RESEARCH 

The  path  tracking  scheme  developed  in  this  research  is  a  simple  algorithm.  This  simple 
nature  provides  ample  opportunities  to  improve  or  expand  the  system.  The  possible 
improvements  can  be  conducted  mainly  in  two  areas,  the  mathematical  model,  and  the 
transitioning  scheme. 

We  developed  the  algorithm  primarily  through  experimentation.  That  is  we  attempted 
to  find  a  valid  control  method  through  hunches,  and  gut  instincts.  This  is  not  the  most 
appropriate  means,  and  it  usually  does  not  yield  the  best  results.  In  deriving  our 
mathematical  model  we  included  assumptions  to  ensure  a  simple  control  function.  In 
addition,  the  output  of  the  system  is  not  precisely  our  desired  output.  A  very  slight 
oscillation  exist  in  simulations  conducted  with  the  value  of  Sq  less  than  one.  Although  the 

magnitude  of  the  oscillation  is  on  the  order  of  1  O'6,  it  may  indicate  a  problem  with  out 
method.  Therefore,  for  these  reasons  it  may  be  beneficial  to  invest  further  research  into  a 
similar  mathematical  model.  This  may  provide  a  solid  mathematical  approach,  which 
produces  superior  results  to  those  presently  attained. 

The  second  area  which  could  be  improved  upon  is  the  transitioning  scheme.  We 
developed  our  transitioning  scheme  primarily  as  a  result  of  the  limits  of  oo'  resources,  both 
time  and  money.  With  greater  resources  available  we  could  investigate  whether  an 
expanded  transitioning  scheme  would  be  feasible  and  economical.  If  the  system  merited 
improvement,  we  could  expand  the  transitioning  scheme  by  taking  more  factors  into 
account  m  determining  the  appropriate  transitioning  distance.  The  goal  would  be  a  fully 
dynamic  scheme,  which  provides  the  safest,  most  efficient  transitions  possible. 

Other  research  can  be  accomplished  by  expanding  the  present  algorithm.  The 
algorithm  could  be  expanded  by  expanding  the  systems’  applicability,  and  functionality. 
The  applicability  of  the  system  could  be  expanded  by  increasing  the  scope  of  possible 
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reference  paths.  Thus,  we  could  implement  a  system  in  which  a  robot  could  path  track  along 
any  given  planar  curve,  including  ovals,  parabolas,  and  cubic  spirals.  This  would  be 
extremely  useful  in  developing  a  path  tracking  scheme  based  on  voronoi  boundaries  within 
an  environment.  Another  possibility  is  to  expand  the  algorithm  to  a  three  dimensional 
system.  This  could  be  useful  in  motion  control  of  an  unteathered  submarine,  or  a  mobile 
drone.  These  options  can  be  accomplished  by  manipulating  the  present  motion  control 
aspects  of  our  system  and  implementing  them  in  other  areas. 

Another  research  area  which  could  be  started  in  conjunction  with  our  work  is  obstacle 
avoidance.  The  path  tracking  algoritlun  lays  the  groundwork  for  a  possible  obstacle 
avoidance  algorithm  for  autonomous  vehicles.  This  is  by  far  the  most  significant  area  of 
possible  future  tesearch.  The  path  tracking  algoritlun  is  simple,  and  provides  a  simple 
reliable  means  to  control  motion  of  a  vehicle.  This  is  ideal  for  an  obstacle  avoidance 
scheme.  The  gioundwork  is  established,  with  further  work  along  these  lines  an  effective 
means  to  transit  unknown  environments  could  be  close  at  hand. 
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APPENDIX  A 


To  test  the  correctness  of  our  control  equation,  we  shall  compare  the  results  of  the 
output  of  our  simulator  with  that  of  an  assumption  free  equation  based  on  our  system.  To 
do  this  we  shall  use  equation  (3.22)  and  solve  for  the  coefficients  A,  B,  and  C.  Let’s  look 
at  equation  (3.22)  and  its’  first  and  second  derivative. 

y  =  ( y -v2  +B.X  +  C)  £~kx  (A.l) 


y  =  (Ax  +  B-  y.\2  -  kBx  -  kc)  Ckx 

(A. 2) 

n 

y  =  {A  -  kA.x-  kB  - 

-  k  (Ax  +  B-  -  kBx  -  kC) )  e~fcv 

(A.3) 

With  these  equations  we  can  solve  for  the  coefficients. 

= 

o  -  y0  c  -  y0 

(A.4) 

/ 

yv  =  o  =  tan0()  B  =  £y0+taneo 

(A.5) 

/ 

y 

A  -  2kB  +  A2C 

,  2  3/Z 

,  3/2  “  K0 

(A.6) 

d  +  (y)  ) 

(1  +  tam0o) 

'.V  =  0 


A  =  2k  ( tanQy  +  Ay0)  -/:2y0  +  K0|cose0|3  (A.7) 

To  find  the  specific  solution  for  the  coefficients  we  use  the  initial  position  of  the  robot 
for  a  given  problem.  To  simplify  the  problem  we  shall  choose  a  problem  in  which  the  robots 
initial  .v  coordinate  is  equal  to  zero.  Thus,  we  shall  solve  the  coefficients  for  the  problem  of 
the  robot  merging  from  an  initial  position  Pi  =  (0,  1,  0,  0)  onto  the  x-axis.  Now,  we  can 
calculate  the  actual  coefficients  for  this  problem.  Using  equation  (A.4)  we  solve  for  C. 

C  =  y0  =  1  (A. 8) 

Now,  we  solve  for  B  using  equations  (A. 5)  and  (A.8). 
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B  =  A-  ( 1)  +  tan  (0)  =  k 


(A  .9) 


And  the  value  of  A  is  calculated. 

A  =  2k  (k)  -A-2(l)  +  (0)  |1|3  =  k2  (A.  10) 

Since,  we  have  assigned  the  value  of  k  to  be  equal  to  the  inverse  of  Sq,  we  have  all  the 
infonnation  to  plot  the  desired  path  free  from  our  earlier  assumptions.  Thus,  we  established 
an  algorithm  which  would  plot  the  following  function. 

k2  i 

v  =  (j.x  +  k\+  1  )£"*'■  (A.ll) 

Thus  the  output  of  this  equation  was  plotted  in  conjunction  with  the  output  of  our  simulator 
for  various  values  of  Sq.  The  results  of  both  outputs  clearly  show  the  degree  of  difference 
between  our  simulator  and  the  assumption  free  output. 
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APPENDIX  B 


In  deriving  our  transition  distance  function,  TD,  we  conducted  numerous  simulations 
of  transitioning  between  paths.  The  objective  was  to  find  the  minimum  distance  required  to 
complete  a  specific  turn  angle.  We  also  conducted  these  experiments  to  detennine  the  effect 
varying  the  value  of  So  had  on  the  minimum  distance.  The  results  where  reported  in  Table 
1 .  and  the  graphical  results  can  be  found  in  figure  22  to  32. 

hi  conducting  the  minimum  transitioning  distance  simulation  we  found  a  problem  with 
the  output  of  our  algorithm.  The  objective  of  the  testing  was  to  find  the  minimum  distance 
to  complete  the  turn  without  any  oscillations  or  crossing  of  the  reference  path.  However,  as 
we  conducted  the  experiment  we  found  that  this  requirement  was  impossible  to  meet  in 
some  circumstances.  As  we  decreased  the  value  of  Sq  we  found  that  a  small  degree  of 

oscillation  was  present,  regardless  of  how  much  we  increased  the  transitioning  distance. 
This  oscillation  was  present  when  the  value  of  Sy  was  0.5,  0.25,  and  0.125.  The  oscillation 

was  a  single  crossing  of  the  x-axis  on  the  order  of  10'6.  Therefore,  we  regarded  this 
oscillation  as  insignificant,  and  proceeded  with  our  testing.  However,  in  the  cases  where 
the  oscillation  existed  we  detennined  the  minimum  distance  as  the  point  where  this  slight 
oscillation  first  appears. 

The  output  of  our  testing  was  four  individual  paths  for  each  specific  turn  angle.  All  of 
the  paths  were  plotted  on  the  same  graph  for  easy  comparison.  The  paths  can  be 
distinguished  from  one  another  since  those  with  the  widest  and  slowest  turns  have  the 
higher  values  for  Sq.  Also  included  on  the  graph  was  a  dotted  directed  line,  which  provides 
the  vehicle's  initial  reference  orientation.  The  ledger  found  in  the  lower  right-hand  comer 
of  each  graph  tell  the  necessary  initial  position  of  the  vehicle  to  complete  the  turn  for  a 
given  Sq  value. 
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APPENDIX  C 


/*  The  header  file  used  in  conjunction  with  the  path  tracking 
algorithm  of  pathld.c  */ 


/*  Constants  needed  within  the  program.  */ 

#define  PI  3.14159265 

♦define  DPI  6.28318530 

♦define  RAD  57.2957795  /*  180  /  PI  */ 

♦define  DELTA_TIME  0.001 

♦define  MAXSTRING  80 

♦define  VELOCITY  10.0 

♦define  MAX_DELTA_KAP P A  1.0 

♦define  N  10 


/*  Structure  for  reference  paths,  vehicle's  position,  image, 
and  the  intersection  point  between  paths.  */ 
typedef  struct  { 
double  x; 

double  y; 

double  theta; 

double  kappa; 

}  CONFIG; 


/*  Global  variables.  */ 
typedef  struct  { 
double  xO; 

double  y0; 

}POINT; 


/*  Procedures  defined  within  the  program.  */ 
CONFIG  current_config,  intersect; 

FILE  *out_path,  *info_file; 

CONFIG  path_array [N] ; 

POINT  intersect; 
int  ITERATIONS; 
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double 

CONFIG 

CONFIG 

void 

void 

CONFIG 

void 

void 

CONFIG 

double 

CONFIG 

double 

void 

double 


i_x°,  i_yO,  dkappa,  delta_d,  DIST_CONSTANT; 


init ial_configuration () ; 
initial__reference  ()  ; 
initialize_parameters () ; 
initialize_f iles  () ; 
intersection_point  () / 
print_to_file () / 
update_velocity ()  ; 
update__configuration  ()  ; 
calc_kappa () ; 
calc_image  ( ) ; 
calc_kappa_dot  ( )  / 
time_to_transition ( )  ; 
norm  ( )  ; 
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#include  'stdio.h' 
# include  "math.h" 
#include  "path.h" 


main  ( ) 

{ 

int  path  nun  =  0; 

int  done  =  0; 

double  dkappa_ds  ~  0.0,  speed,  delta_dist,  aa,  bb,  cc; 
CONFIG  path,  image; 

vehicle  =  initial_configuration ()  ; 
path  =  initial_ref -rence ( )  ; 
initialize_jparameters  (&aa,  &bb,  &cc)  ; 
initialize_f iles (path) ; 

intersect  =  intersection_point (path_num) ; 
do  { 

print_to_f ile (image  path,  dkappa_ds) ; 
update_speed (Sspeed,  &delta_dist) ; 
update_image (& image,  vehicle,  path); 
delta_d  =  de It a_di stance (vehicle,  image); 
dkappa_ds  =kappa_dot (vehicle,  image,  delta_d,  delta_dist, 

aa,  bb,  cc) ; 

update_conf iguration (^vehicle,  dkappa_ds,  delta_dist); 
time_to_transition (Spath,  &path_num,  &done,  image); 

)  while  ( ! (done) ) ; 


CONFIG  initial_conf iguration  () 

{ 

CONFIG  vehicle; 
double  dgre; 

printf  ( ''Enter  the  robots  current  configuration  (X  Y  DEGREES 
KAPPA)  :  " )  ; 

scanf ( "%lf%lf %lf%lf " , & vehicle . x, & vehicle . y, Sdgre, 

& vehicle . kappa'  ; 


vehicle . theta  =  norm(dgre  /  RAD); 
return (vehicle)  ; 


CONFIG  initial_reference ( ) 

{ 

double  degree,  kpa; 
int  i,  num_of_paths; 

print f ("How  many  reference  paths  do  you  desire  in  the  robots 
motion:  ") ; 

scanf  ("%d"  ,  &num_of_jpaths)  ; 

for  (i=0;  i<  num_ofjpaths ;  ++i)  { 

printf ("\n%s%d%s",  "Enter  the  equation  for  number  ", 

(i+1) , "  reference  path  (X  Y  THETA  KAPPA)"); 
scanf ("%lf%lf%lf%lf",  &path_array [i] . x,  &path_array [i] . y, 
Sdegree,  &kpa) ; 

path_array [i] .theta  =  norm(aegree  /  RAD); 
path_array [i] . kappa  =  kpa; 

} 

path_array [num_of_paths ] . x  =  0.0; 
path_array [num_of_paths ] .y  =  0.0; 
path_array  [num__of_jpaths  ]  .  theta  =  6.28; 
path_array  [num_of_jpaths]  .  kappa  =  0.0; 
return (path_array [ 0 ]  )  ; 


void  init ialize_parameters (aa,  bb,  cc) 
double  *aa,  *bb,  *cc; 

{ 

double  kk; 

printf ("What  is  the  desired  value  of  the  distance 
constant:  ") ; 

scanf ("%lf",  &DIST_CONSTANT) ; 
kk  =  1.0  /  D I S  T_CON S  TANT ; 

*aa  =  3.0  *  kk; 
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*bb  =  *aa  *  kk; 

*cc  =  *bb  *  kk  /  3.0; 


void  initialize_files (path) 

CONFIG  path; 

{ 

char  name [MAXSTRING] ,  namel [MAXSTRING] ; 

printf("What  is  the  name  of  the  graph  output  file?  ") ; 
scanf  ("%s",  name) ; 

printf(”What  is  the  name  of  the  info  output  file?  ”) ; 

scanf  {"%s",  namel); 

info_file  =  fopen (namel,  "w") ; 

out_path  =  fopen (name,  ”w") ; 

fprintf (out_path,  "%s% . If \n%s\n" ,  "#cs  ",  0.5,  "#lg  2"); 
fprintf  (out_path,  " %s% . 2f%s\n" , 

"#leg  \ "distance  constant=",  DIST_CONSTANT,  "\"") 
fprintf (out_path,  "%s%27s\n",  "#leg  \" (X, Y, Theta, Kappa) " , 
" (Xr, Yr, Thetar, Kappar) \" ") ;  fprintf (out_path, 
"%s%.lf,  %.lf,  %.lf,  %.lf%s%5s%.lf,  %.lf,  %.lf, 
%.lf%s\n",  "#leg  \"(",  vehicle. x,  vehicle. y, 
vehicle. theta*RAD,  vehicle . kappa, ")","(", 
path. x, path. y, path. theta  *  RAD,  path. kappa, ") \"") 


void  print_to_f ile (image,  path,  dkappa_ds) 
CONFIG  image,  path; 
double  dk  app  a_ds ; 

{ 


printf  (  "%lf  ",  vehicle.x); 

printf (  "  %lf\n  ",  vehicle.y); 

fprintf  (out_path,  "%lf  ",  vehicle. x) ; 

fprintf (out_path,  "  %lf\n  ",  vehicle.y); 

fprintf (info_file,  "%s%lf  ",  "x=",  vehicle.x); 

fprintf (info_f ile,  "%s%lf  ",  "y=",  vehicle.y); 

fpr j nt f (inf o_file,  "%s%lf  ",  "theta=",  vehicle .theta); 
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} 


fprintf (info_file, 
fprintf (info_file, 
fprintf (info_file, 
fprintf (info_file, 
fprintf (info_file, 
fprintf (info_file, 
fprintf (info_file, 
fprintf (info_file, 
fprintf (info_file, 
fprintf (info_file, 
fprintf (info_file, 
fprintf (info  file, 


"%s%lf\n  ",  "kappa=",  vehicle . kappa) ; 
"%s?.lf  ","itheta=",  image . theta) ; 
"%s%.5f  ",  "image_>:=",  i_xO) ; 

"%s%.5f  ",  "image_y=",  i_yO) ; 

"%s%lf\n  ",  "close_dist=" ,  delta_d) ; 
"%s%lf  " ,  "dkappa_ds=" ,  dkappa_ds) / 
"%s%lf  " , "inter_x=" ,  intersect . x) ; 
"%s%lf\n  ", "inter_y=" ,  intersect . y) ; 
"%s%lf  ",  "path.x=",  path.x) ; 

"%s%lf  ","path.y=",  path.y); 

"%s%lf  ","path.t=",  path. theta) / 
"%s%lf\n\n  ","path.k=",  path. kappa); 


void  update_speed (speed,  delta_dist) 
double  * speed,  *delta_dist; 

{ 

^ speed  =  VELOCITY; 

*delta_dist  =  DELTA_T I ME  *  *speed; 


double  delta_distance (vehicle,  path) 

CONFIG  vehicle,  path; 

{ 

double  distance; 

distance  =  (- (vehicle. x  -  path.x)  *  (path. kappa  * 

(vehicle. x  -  path.x)  +  2  *  sin (path . theta) )  - 
(vehicle. y  -  path.y)  *  (path. kappa  * 

(vehicle. y  -  path.y)  -  2  *  cos (path. theta) ) )  / 
(1  +  sqrt  (pow  (path. kappa  *  (vehicle . -  path.x) 

+  sin  (path. theta) ,  2.0)  +  pow  (path. kappa  * 
(vehicle. y  -  path.y)  -  cos  (path. theta) ,  2.0))); 

return (distance) ; 

} 
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void  update_image (image,  vehicle,  path) 

CONFIG  *image,  vehicle,  path; 

{ 

double  radius,  gamma,  close_dist; 

POINT  origin; 

if  (path. kappa  ==0.0)  { 

close_dist  =  (( (vehicle. y  -  path.y)  *  cos (path. theta)  ) 
(  (vehicle. x  -  path.x)  *  sin (path. theta)  ))  ; 
(’"'image). x  =  vehicle. x  +  close_dist  *  sin  (path,  theta) 
(*image) .y  =  vehicle. y  -  close_dist  *  cos (path. theta) 
(*image) .theta  =  path. theta; 

(* image) .kappa  =  path. kappa; 

) 

else  { 

radius  =  (1.0  /  path. kappa) ; 

origin. xO  =  path.x  -  radius  *  (sin  (path . theta) ) ; 
origin. yO  =  path.y  +  radius  *  (cos (path. theta) ) ; 
gamma  =  atan2 (vehicle . y  -  origin. yO, 
vehicle. x  -  origin. xO) ; 

(*image) .x  =  origin. xO  +  fabs(radius)  *  (cos (gamma) ) ; 
(*image) .y  =  origin. yO  +  fabs(radius)  *  (sin (gamma) ) ; 
<*image) .theta  =  norm(gamma  +  (PI/2) *  (path . kappa/ 
fabs  (path. kappa) ) ) ; 

(* image) .kappa  =  path. kappa; 

} 

i_x0  =  (* image ).x; 
i_y0  =  (* image) .y; 


double  kappa_dot (vehicle,  image,  delta_d,  delta_dist, 
aa,  bb,  cc) 

CONFIG  vehicle,  image; 

double  delta_dist,  delta_d,  aa,  bb,  cc; 

{ 

double  delta_kappa,  dkappal; 
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dkappal  =  -aa  ♦  (vehicle • kappa  -  image. kappa) 

-bb  *  (norm (vehicle . theta  -  image. theta) ) 
-cc  *  delta_d; 

delta_kappa  =  dkappal  *  delta_dist; 
return (delta_kappa) ; 

} 


void  update_configurat ion (vehicle,  dkappa_ds,  delta_dist) 

CONFIG  * vehicle; 

double  dkappa_ds,  delta_dist; 

{ 

double  delta_theta,  delta_distl,  kappa; 
double  epsilon  =  0.00001; 

kappa  =  ( ^vehicle) . kappa  +  dkappa_ds; 
delta_theta  =  delta_dist  *  kappa; 
delta_distl  =  delta_dist; 
if  (fabs (delta_theta)  <=  epsilon)  { 

delta_distl  =delta_dist  ♦  (sin  (delta_theta/2)  / 
(delta_theta/2) ) ; 

} 

(♦vehicle) .x  +=  (cos ( (*vehicle) .theta  +  delta_theta  /  2.0) 

♦  delta_distl) ; 

(♦vehicle) .y  +=  (sin ( (♦vehicle) .theta  +  delta_theta  /  2.0) 

♦  delta_distl) ; 

(♦vehicle) .theta  =  norm ( (*vehicle) .theta  +  delta_theta) ; 
(♦vehicle) .kappa  =  kappa; 


void  time_to_transition (path,  path  num,  done,  image) 

CONFIG  *path,  image; 
int  *path_num,  *done; 

{ 

double  distance,  turn_angle,  TDist; 

if  (path_array  [  *path__num+l  ] .theta  ==  6.28) { 

if  ( ( (fabs (delta_d)  <  0.00001)  &&  (fabs ( (*path) . kappa  - 
vehicle . kappa)  <=  0.00001)  &&  (fabs (norm  (vehicle. theta  - 
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image. theta) )  <=  0.00001))  ||  (ITERATIONS  >  2000)) 

*done  =  1 ; 
else  { 

♦done  =  0; 

ITERATIONS  +=  1; 

) 

} 

else  { 

turn_angle  =  intersect .theta ; 

distance  =  sqrt (pow (image . x  -  intersect. x,  2.0)  + 
pow (image. y  -  inter sect. y,  2.0)); 

TDist  =  (2.4  *  DIST_CONSTANT  +  0.3)  *  (1/(1-  pow  (turn_angle 
/  PI,  4.0) ) ) ; 
if  (distance  <  TDist  )  { 

♦path  =  path_array [ ♦path_num  +  1  ]  ; 

*path_num  +=  1 ; 

intersect  =  intersect ion_jpoint  (*path_num)  ; 

♦done  =  0; 

fprintf  (out_jpath,  "%s%  .  If ,  %.lf,  %.lf,  % .  lf%s%5s% .  If ,  %.lf, 
%.lf,  % .  If  %s\n" ,  "#leg  \”(",  vehicle. x,  vehicle. y, 
vehicle. theta+RAD,  vehicle. kappa, ") ”, " (”, 

(♦path) .x, (*path) .y,  (*path) .theta  *  RAD, 

(♦path) .kapra, ") \"") ; 

) 

else 

♦done  =  0; 

} 

} 


CONFIG  intersection_point (path_num) 
int  path__num; 

{ 

double  dist_refs,  ref_orient,  beta,  alpha,  sigma,  inter_d, 
distance,  distancel,  phi,  intersect_orient ; 

POINT  imagel,  origin; 

CONFIG  inter; 

if  (path_array [path_num+l] .theta  ==  6.28)  { 

inter . x=path_array [path_num] . x  + 

1000*cos (path_array (path_num] .theta) ; 
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inter  .  y=pat!i_arj.;~y  ; oath  Rum]  .y  + 

100C*sin array [path_num] .theta) ; 

I 

else  if  (  (path_array  [p?  :  numj. kappa  ==  0.0)  && 

(path_array [path_num+l ]. kappa  ==  0.0))] 
dist__refs=sqrt  (pow  (path_array  [path_num  +l].x  - 
path_array [path_num] . x,  2.0)  + 
pew (path_array [path_num  +1] .y  - 
path_array [path_numj . y ,  2.0)); 
ref_orient  =  atan2 (path_array [path_num  +  l].y  - 
path_array [path_num] .y, 
path_array [path_num  +1] . x  - 
path__array  [path_num]  .  x)  ; 
beta  =  path_array [path_num] . theta  -  ref_orient; 
alpha  =  norm(PI  -  ref_orient)  -  path_array  [path_num  + 

1 ] . theta; 

sigma  =  norm(PI  -  beta  -  alpha) ; 

inter_d  =  dist_refs  *  (sin(alpha)/sin(sigma)); 

inter . x=path_array [path_num] . x  + 

inter_d*cos (path_array [path_num] .theta) ; 
inter . y=path_array [path_num] .y  + 

inter_d*sin (path_array [path_num] .theta) ; 
inter. theta  =  norm (path_array [path_num  +  1] .theta  - 
path_array [path _num] .theta) ; 

} 

else  if (path_array [path_num] .kappa  ==  0.0)  { 

origin. xO  =  path_array [path_num+l] . x  -  (1/ 
path_array [path_num+l] .kappa)  * 

(sin (path_array [path_num+l] .theta) ) ; 
origin. yO  =  path_array [path_num+l] .y  +  (1/ 
path_array [path_num+l ]. kappa)  * 

(cos (path_array [path_num+l ] .theta)) ; 
distance  =  (origin. yO  -  path_array [path_num] . y )  * 

cos (path_array [path_num] . theta)  -  (origin. xO  - 
path_array [path_num] .x)  * 
sin  (path_array Ipath_num] .theta) ; 
distancel  =sqrt  (pow  (l/'path_array  [path_num+l  ]  . kappa, 2. 0)- 
pow (distance, 2.0)); 
imagel.xO  -  origin. xO  +  distance  * 

sin  (path_array  [path_num]  .theta)  .* 
imagel.yO  =  origin. yO  -  distance  * 

cos  (path_array  (path__num]  .theta)  ; 
inter. x=  imagel.xO  -  distancel 


87 


*cos (path_array [path_num] .theta) ; 
inter. y=  imagel.yO  -  distancel 

*sin (path_array [path_num] .theta) ; 
phi  =  atan2 (inter .y  -  origin. yO,  inter. x  -  origin. xO) ; 
intersect_orient  =  norm (phi  +  (PI/2)  * 

(path_array [path_num  +1] .kappa  / 
fabs (path_array [path_num  +  1] .kappa) ) ) ; 
inter. theta  =  norm (intersect_orient  - 
path_array [path_num] .theta) ; 

) 

else  { 

origin. xO  =  path_array [path_num] . x  -  (1/ 
path_array [path_num] .kappa)  * 

(sin (path_array [path_num] .theta) ) ; 
origin. yO  =  path_array [path_num] . y  +  (1/ 
path_array [path_num] . kappa)  * 

(cos (path_array [path_num] .theta) ) ; 
distance  =  (origin. yO  -  path_array [path_num+l] . y)  * 

cos (path_array [path_num+l ] .theta)  -  (origin. xO  - 
path_array [path_num+l ) . x)  * 
sin (path_array [path_num+l ) .theta) ; 
distancel  =  sqrt (pow (l/path_array [path_num] . kappa, 2 . 0) - 
pow  (distance.  2.0)); 
imagel.xO  =  origin. xO  +  distance  * 

sin (path_array [path_num+l] .theta) ; 
imagel.yO  =  origin. yO  -  distance  * 

cos (path_array [path_num+l] .theta) ; 
inter. x=  imagel.xO  +  distancel 

*cos  (path_array  [path_num+l]  .theta)  ; 
inter. y=  imagel.yO  +  distancel 

*sin (path_array [path_num+l] .theta) ; 
phi  =  atan2 (inter .y  -  origin. yO,  inter. x  -  origin. xO) ; 
intersect_orient  =  norm (phi  +  (PI/2)  * 

(path_array [path_num] . kappa  / 
fabs (path_array [path_num] .kappa) ) ) ; 
inter. theta  =  norm(-  intersect_orient  + 
path_array [path_num  +  1 ]. theta); 

} 

return (inter) ; 

} 


ss 


double  norm (angle) 
double  angle; 

/ 


while  ((angle  >  PI)  ||  (angle 
if  (angle  >  PI) 
angle  -=  DPI; 
else 

angle  +=  DPI; 

} 

return  (angle) ; 


} 


APPENDIX  D 


We  propose  a  path  tracking  algorithm  which  is  applicable  to  any  autonomous  vehicle. 
This  algorithm  would  be  operable  in  conjunction  with  the  present  motion  control  software 
system  MML  for  Y amabico.  An  overview  of  the  present  locomotion  functions  is  presented 
by  Kanayama  [Ref.  4],  which  seconds  as  Yamabico’s  user  manual.  This  reference  gives  the 
reader  a  clear  understanding  of  the  MML  software  environment,  and  the  available  motion 
control  commands.  The  modification  which  would  be  necessary  to  combine  the  two 
independent  systems  was  explained  in  chapter  VII.  In  this  appendix  we  would  like  to 
explain  the  additions  necessary  to  the  library  functions  in  the  MML  language  by  expanding 
the  user's  manual. 

Tire  changes  which  are  necessary  are  concerned  exclusively  with  the  sequential 
locomotion  functions,  such  as  move  and  stop.  To  append  the  users  manual  we  must  simply 
add  three  locomotion  functions  to  the  system.  These  three  commands  shall  allow  the  user 
to  intersperse  the  functional  features  of  the  path  tracking  algorithm  with  the  established 
point  to  point  motion  control  scheme.  The  added  commands  will  be  labeled  as  follows; 
path,ppath,  and  spath.  It  is  our  hope,  that  by  incorporating  both  systems  together  with  as 
few  as  possible  additional  commands,  we  can  maintain  the  entire  MML  environment.  In 
addition,  we  feel  this  will  greatly  expand  the  robot's  functionality  and  versatility  without 
complicating  the  operating  procedure  of  the  robot.  Let’s  take  a  in-depth  look  at  each  new 
sequential  locomotion  function. 

PATH  TRACKING  LOCOMOTION  FUNCTIONS: 

MOVE  PATH 

SYNOPSIS:  path{  c); 

CON  FIGURATION  *c; 
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DESCRIPTION: 


This  function  moves  the  robot  from  the  robot’s  current 
configuration  p;  =  (.Vj,  vj,  Gj,  Kj)  onto  the  directed 
reference  path  =  (.v,  v,  0,  k).  The  present  speed  and 
acceleration,  vc  and  ac,  are  used  for  the  motion.  The 
robot’s  motion  should  smoothly  merge  onto  the 
reference  path  and  continue  to  track  along  the  path 
indefinitely  or  until  another  motion  function  is 
encountered.  The  only  acceptable  command  following  a 
path()  is  another  path  tracking  locomotion  function, 
or  an  immediate  function.  The  locomotion  function 
which  follows  this  command  must  consist  of  a  path 
which  intersects  the  present  desired  path.  If  the  two 
consecutive  paths  do  not  intersect,  the  robot  stays  on  the 
current  reference  path. 

ERROR:  If  the  command  following  path()  is  not  an  acceptable 

option,  the  robot  stops  and  an  error  code  is  returned. 

SEE  ALSO:  ppath(),  spath(),  speed(),  acc(),  movei(). 

MOVE  PARTIAL  PATH 

SYNOPSIS:  ppath{  c); 

CONFIGURATION  *c; 

DESCRIPTION:  This  function  is  a  variation  of  the  path  function.  It  moves 

the  robot  from  the  robot’s 

current  configuration  p;  =  (,\j,  Vj,  0j,  Kj)  onto  the 
desired  reference  path.  The  present  speed  and 
acceleration,  vc  and  ac,  are  used  for  the  motion.  The 
robot’s  should  smoothly  merge  onto  the  desired 
reference  path  and  continue  to  track  along  the  path  until 
its  image  reaches  c  =  (,v,  v,  0,  k).  At  this  point  control 
goes  to  the  next  path  function. 

If  the  destination  point  c  is  the  same  as  the  current 
nominal  point,  the  robot  stops  and  an  error  code  is 
returned. 

path(),  spathQ,  speed(),  acc(),  movei(),  move(), 
stop(). 


ERROR: 


SEE  ALSO: 
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STOP  PATH 


SYNOPSIS: 

DESCRIPTION: 

SEE  ALSO: 

SPECIFYING  S0 
SYNOPSIS: 

DESCRIPTION: 

ERROR: 


spathic ); 

CONFIGURATION  *c; 

This  function  is  a  variation  of  the  path()  function.  It 
moves  the  robot  from  the  robot’s  current  configuration 
Pi  =  (A‘i>  }'i>  ©i.  Kj)  onto  the  desired  reference  path.  The 
present  speed  and  acceleration,  vc  and  ac,  are  used  for 
the  motion.  The  robot  should  smoothly  merge  onto  the 
desired  reference  path  and  continue  to  track  along  the 
path  until  its  image  stops  at  c  =  (.v,  y,  9,  k).  At  this  point 
the  robot  switches  to  the  STOP  state.  In  this  state  the 
robot  can  complete  any  command  of  either  system. 

path().  ppath().  speed(),  acc(),  movei(),  move(). 
stop(). 


dist_const(x)\ 
double  x; 

This  function  allows  the  user  to  adjust  the  value  of  So 
for  a  particular  maneuver.  The  command  can  only  be 
used  while  the  robot  is  in  the  STOP  state. 

If  the  user  inputs  a  negative  value  for  the  distance 
constant,  the  robot  stops  and  an  error  code  is  returned. 
If  this  command  is  given  while  the  robot  is  not  in  the 
STOP  state,  the  robot  shall  stop  and  an  error  code 
returned. 
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